From 71086bb71ef53348bd607d323e7cebaa52b5679f Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Tue, 30 Apr 2024 16:41:08 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E9=83=A8=E5=88=86=E7=94=B5?=
 =?UTF-8?q?=E6=9C=BA=E5=81=9C=E8=BD=AC=E5=92=8C=E5=8F=8D=E8=BD=AC=E7=9A=84?=
 =?UTF-8?q?bug?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 modules/motor/DJImotor/dji_motor.c | 2 +-
 modules/motor/HTmotor/HT04.c       | 7 ++++---
 modules/motor/HTmotor/HT04.h       | 1 +
 modules/motor/LKmotor/LK9025.c     | 7 ++++---
 modules/motor/LKmotor/LK9025.h     | 2 +-
 5 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c
index e9345cc..bc215d4 100644
--- a/modules/motor/DJImotor/dji_motor.c
+++ b/modules/motor/DJImotor/dji_motor.c
@@ -300,7 +300,7 @@ void DJIMotorControl()
 
         // 若该电机处于停止状态,直接将buff置零
         if (motor->stop_flag == MOTOR_STOP)
-            memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
+            memset(sender_assignment[group].tx_buff + 2 * num, 0, sizeof(uint16_t));
     }
 
     // 遍历flag,检查是否要发送这一帧报文
diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c
index fdfad15..eec1167 100644
--- a/modules/motor/HTmotor/HT04.c
+++ b/modules/motor/HTmotor/HT04.c
@@ -121,7 +121,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
     Daemon_Init_Config_s conf = {
         .callback = HTMotorLostCallback,
         .owner_id = motor,
-        .reload_count = 5, // 20ms
+        .reload_count = 5, // 50ms
     };
     motor->motor_daemon = DaemonRegister(&conf);
 
@@ -155,6 +155,9 @@ __attribute__((noreturn)) void HTMotorTask(void const *argument)
     while (1)
     {
         pid_ref = motor->pid_ref;
+        if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
+            pid_ref *= -1;
+
         if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
         {
             if (setting->angle_feedback_source == OTHER_FEED)
@@ -186,8 +189,6 @@ __attribute__((noreturn)) void HTMotorTask(void const *argument)
         }
 
         set = pid_ref;
-        if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
-            set *= -1;
 
         LIMIT_MIN_MAX(set, T_MIN, T_MAX);
         tmp = float_to_uint(set, T_MIN, T_MAX, 12);
diff --git a/modules/motor/HTmotor/HT04.h b/modules/motor/HTmotor/HT04.h
index 1ad61f5..caa8957 100644
--- a/modules/motor/HTmotor/HT04.h
+++ b/modules/motor/HTmotor/HT04.h
@@ -11,6 +11,7 @@
 #define CURRENT_SMOOTH_COEF 0.9f
 #define SPEED_BUFFER_SIZE 5
 #define HT_SPEED_BIAS -0.0109901428f // 电机速度偏差,单位rad/s
+#define TORQUE_COEF_HT 3.5  // 扭矩系数,单位N.m/A
 
 #define P_MIN -95.5f // Radians
 #define P_MAX 95.5f
diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c
index c9fadbc..b98c018 100644
--- a/modules/motor/LKmotor/LK9025.c
+++ b/modules/motor/LKmotor/LK9025.c
@@ -104,6 +104,8 @@ void LKMotorControl()
         measure = &motor->measure;
         setting = &motor->motor_settings;
         pid_ref = motor->pid_ref;
+        if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
+            pid_ref *= -1;
 
         if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
         {
@@ -132,9 +134,8 @@ void LKMotorControl()
             pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
         }
 
-        set = pid_ref;
-        if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
-            set *= -1;
+        set = (int16_t)pid_ref;
+
         // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
         memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
 
diff --git a/modules/motor/LKmotor/LK9025.h b/modules/motor/LKmotor/LK9025.h
index a5cef5a..10a32ba 100644
--- a/modules/motor/LKmotor/LK9025.h
+++ b/modules/motor/LKmotor/LK9025.h
@@ -15,7 +15,7 @@
 #define SPEED_SMOOTH_COEF 0.85f
 #define REDUCTION_RATIO_DRIVEN 1
 #define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
-#define CURRENT_TORQUE_COEF_LK 0.003645f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
+#define CURRENT_TORQUE_COEF_LK 0.00512f  // 电流设定值转换成扭矩的系数，这里对应的是16T
 
 typedef struct // 9025
 {