修复电机反转bug，添加部分注释

.vscode/launch.json

@@ -47,8 +47,8 @@
             "liveWatch": {
                 "enabled": true,
                 "samplesPerSecond": 4
-            }
-            //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口,若daplink使用jlinkGDBserver启动,需要先开始调试再打开log
+            },
+            // "preLaunchTask": "log", // 调试时同时开启RTT viewer窗口,若daplink使用jlinkGDBserver启动,需要先开始调试再打开log
             // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖
         },
         {

.vscode/settings.json

@@ -1,5 +1,8 @@
 {
     "files.associations": {
-        "stm32f4xx_hal_def.h": "c"
+        "stm32f4xx_hal_def.h": "c",
+        "general_def.h": "c",
+        "stdlib.h": "c",
+        "bsp_can.h": "c"
     }
 }

.vscode/tasks.json

@@ -36,9 +36,9 @@
             "command":"JlinkRTTClient",
             "args": [],
             "problemMatcher": [],
-            // "dependsOn":[
-            //     "build task", // 可以添加多个.
-            // ]
+            "dependsOn":[
+                "build task", // 可以添加多个.
+            ]
             // 若使用daplink,则将log任务设置为依赖于jlink launch任务,保证jlink launch任务先于log任务执行
         }
     ]

application/chassis/balance.c

@@ -22,49 +22,54 @@ static float del_t;
 
 /* 底盘拥有的模块实例 */
 static attitude_t *imu_data;
-static RC_ctrl_t *rc_data; // 调试用
+static RC_ctrl_t *rc_data; // 底盘单独调试用
 static Referee_Interactive_info_t my_ui;
 static referee_info_t *referee_data;
-static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // syh
+static Chassis_Ctrl_Cmd_s chassis_cmd_recv; 
 static Chassis_Upload_Data_s chassis_feed;
 static CANCommInstance *ci;
-static SuperCapInstance *cap; // syh
+static SuperCapInstance *cap; 
+
 // 四个关节电机和两个驱动轮电机
 static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
 static LKMotorInstance *l_driven, *r_driven, *driven[2];
+
 // 两个腿的参数,0为左腿,1为右腿
-static LinkNPodParam l_side, r_side; // syh  phi5
+static LinkNPodParam l_side, r_side; 
 static ChassisParam chassis;
+
 // 综合运动补偿的PID控制器
 static PIDInstance steer_p_pid, steer_v_pid;   // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
 static PIDInstance anti_crash_pid, phi5_pid;   // 抗劈叉,将输出以相反的方向叠加到左右腿的上
 static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
-static PIDInstance legdot_pid_l, legdot_pid_r;
 static PIDInstance roll_compensate_pid, rolldot_pid; // roll轴补偿,用于保持机体水平
 
 static Robot_Status_e chassis_status;
 
 void BalanceInit()
 {
-    referee_data = UITaskInit(&huart6, &my_ui);
     rc_data = RemoteControlInit(&huart3);
+    imu_data = INS_Init();
+
+    // 双板通信
     CANComm_Init_Config_s commconf = {
         .can_config = {
-            .can_handle = &hcan1,
+            .can_handle = &hcan2,
             .tx_id = 0x40,
             .rx_id = 0x41},
         .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
         .send_data_len = sizeof(Chassis_Upload_Data_s)};
     ci = CANCommInit(&commconf);
-    imu_data = INS_Init();
 
+    // 超级电容
     SuperCap_Init_Config_s cap_conf = {
         .can_config = {
-            .can_handle = &hcan1,
+            .can_handle = &hcan2,
             .tx_id = 0x302, // todo 电容id
             .rx_id = 0x301}};
     cap = SuperCapInit(&cap_conf);
 
+    // 关节电机
     Motor_Init_Config_s joint_conf = {
         // 写一个,剩下的修改方向和id即可
         .can_init_config = {
@@ -101,6 +106,7 @@ void BalanceInit()
     joint_conf.can_init_config.rx_id = 11;
     joint[RB] = rb = HTMotorInit(&joint_conf);
 
+    // 驱动轮电机
     Motor_Init_Config_s driven_conf = {
         // 写一个,剩下的修改方向和id即可
         .can_init_config.can_handle = &hcan2,
@@ -118,6 +124,7 @@ void BalanceInit()
     driven_conf.can_init_config.tx_id = 2;
     driven[RD] = r_driven = LKMotorInit(&driven_conf);
 
+    // 转向PID
     PID_Init_Config_s steer_p_pid_conf = {
         .Kp = 2,
         .Kd = 1,
@@ -140,6 +147,7 @@ void BalanceInit()
     };
     PIDInit(&steer_v_pid, &steer_v_pid_conf);
 
+    // 抗劈叉
     PID_Init_Config_s anti_crash_pid_conf = {
         .Kp = 8,
         .Kd = 2.5,
@@ -154,6 +162,7 @@ void BalanceInit()
     };
     PIDInit(&anti_crash_pid, &anti_crash_pid_conf);
 
+    // 腿长控制
     PID_Init_Config_s leg_length_pid_conf = {
         .Kp = 450,
         .Kd = 150,
@@ -168,6 +177,7 @@ void BalanceInit()
     PIDInit(&leglen_pid_l, &leg_length_pid_conf);
     PIDInit(&leglen_pid_r, &leg_length_pid_conf);
 
+    // 横滚角补偿
     PID_Init_Config_s roll_compensate_pid_conf = {
         .Kp = 0.0008f,
         .Kd = 0.00065f,
@@ -179,7 +189,7 @@ void BalanceInit()
     };
     PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);
 
-    l_side.target_len = r_side.target_len = 0.23;
+    l_side.target_len = r_side.target_len = 0.23;   // 初始腿长
     chassis.vel_cov = 1000; // 初始化速度协方差
     chassis_status = ROBOT_READY;
     DWT_GetDeltaT(&balance_dwt_cnt);
@@ -195,7 +205,8 @@ static void EnableAllMotor() /* 打开所有电机 */
 
 /* 切换底盘遥控器控制和云台双板控制 */
 static void ControlSwitch()
-{ // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
+{ 
+    // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
     if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline())
     {
         if (rc_data->rc.rocker_l1 < -600)
@@ -220,6 +231,9 @@ static void ControlSwitch()
 
 void BalanceTask()
 {
- 
+    del_t = DWT_GetDeltaT(&balance_dwt_cnt);
+
+    // 切换遥控器控制or云台板控制
+    ControlSwitch();
     
 }

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);

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));
 

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 // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
 
 typedef struct // 9025
 {