改底盘（疯）

application/chassis/chassis.c

@@ -145,10 +145,10 @@ void ChassisInit()
         .motor_type = GM6020,
     };
 
-    chassis_motor_steering_config.can_init_config.tx_id = 1;
+    chassis_motor_steering_config.can_init_config.tx_id = 2;
     motor_steering_rf                                   = DJIMotorInit(&chassis_motor_steering_config);
 
-    chassis_motor_steering_config.can_init_config.tx_id = 2;
+    chassis_motor_steering_config.can_init_config.tx_id = 1;
     motor_steering_lb                                   = DJIMotorInit(&chassis_motor_steering_config);
 
     PID_Init_Config_s chassis_follow_pid_conf = {
@@ -172,11 +172,14 @@ void ChassisInit()
     SuperCap_Init_Config_s cap_conf = {
         .can_config = {
             .can_handle = &hcan2,
-            .tx_id = 0x302, // 超级电容默认接收id
+            .tx_id = 0x001, // 超级电容默认接收id
-            .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
+            .rx_id = 0x100, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
         }};
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
 
+    // （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整
+    SuperCapSetPowerOffset(cap, 5);
+
     // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
     Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
@@ -223,16 +226,43 @@ void ChassisInit()
  */
 static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
 {
-    float rotation = *angle - *last_angle;
+    float target_angle = *angle;
-    ANGLE_LIMIT_360_TO_180_ABS(rotation);
+    float actual_angle = *last_angle;
-    if (rotation > 90) {
+    float rotation = target_angle - actual_angle;
-        *angle -= 180;
+    float norm_rotation = rotation;
+    while (norm_rotation > 180.0f) {
+        norm_rotation -= 360.0f;
+    }
+    while (norm_rotation < -180.0f) {
+        norm_rotation += 360.0f;
+    }
+    float threshold = 90.0f;
+    float target_single=fmodf(target_angle,360.0f);
+    if(target_single<0.0f) target_single+=360.0f;
+
+    float current_single=fmodf(actual_angle,360.0f);
+    if(current_single<0.0f) current_single+=360.0f;
+
+
+    if (norm_rotation > threshold) {
+        int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f);
+        *angle = actual_angle + norm_rotation-180.0f+round_diff*360.0f;
         *speed = -(*speed);
-    } else if (rotation < -90) {
+    }else if(norm_rotation<-threshold) {
-        *angle += 180;
+        int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f);
+        *angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f;
         *speed = -(*speed);
     }
-    ANGLE_LIMIT_360_TO_180_ABS(*angle);
+    // float rotation = *angle - *last_angle;
+    // ANGLE_LIMIT_360_TO_180_ABS(rotation);
+    // if (rotation > 90) {
+    //     *angle -= 180;
+    //     *speed = -(*speed);
+    // } else if (rotation < -90) {
+    //     *angle += 180;
+    //     *speed = -(*speed);
+    // }
+    // ANGLE_LIMIT_360_TO_180_ABS(*angle);
 }
 
 /**
@@ -367,7 +397,17 @@ void ChassisTask() {
             chassis_cmd_recv.wz = 0;
             break;
         case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
-            chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0);
+            //chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0);
+            float single_round_offset = chassis_cmd_recv.offset_angle;
+            if(single_round_offset > 180.0f)
+            {
+                single_round_offset -= 360.0f;
+            }
+            else if(single_round_offset < -180.0f)
+            {
+                single_round_offset += 360.0f;
+            }
+            chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/90;
             break;
         case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
             chassis_cmd_recv.wz = 0.75;