底盘正常移动，小陀螺降速方便调

This reverts commit 69e97cbefa.

application/chassis/chassis.c

@@ -31,7 +31,7 @@
 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
 #if defined(CHASSIS_MCNAMEE_WHEEL)
-//#define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量
+#define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量
 #elif defined(CHASSIS_STEERING_WHEEL)
 #define CHASSIS_WHEEL_OFFSET 30.0f       // 机器人底盘轮子修正偏移量
 //#define CHASSIS_WHEEL_OFFSET 2.0f
@@ -157,12 +157,12 @@ void ChassisInit()
         .Ki                = 0,
         .Kd                = 0.1f, // 0.5
         .DeadBand          = 0.5,
-        // .CoefA             = 0.2,
+        .CoefA             = 0.2,
-        // .CoefB             = 0.3,
+        .CoefB             = 0.3,
-        // .Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
+        .Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
-        // .IntegralLimit     = 50, // 200
+        .IntegralLimit     = 50, // 200
-        .MaxOut            = 50,
+        .MaxOut            = 1200,
-        // .Derivative_LPF_RC = 0.01, // 0.01
+        .Derivative_LPF_RC = 0.01, // 0.01
     };
     PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf);
 
@@ -284,8 +284,7 @@ static void SteeringWheelCalculate()
         vt_rf = 0, vt_lb = 0;
     } else {
         // 生成预计算变量，减少计算量，空间换时间
-        //float w      = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
+        float w      = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
-        float w      = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET ;
         // float temp_x = chassis_vx - w, temp_y = chassis_vy - w;
         // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x-
         // temp_y = chassis_vy + w;                                 // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释
@@ -409,62 +408,36 @@ void ChassisTask() {
             {
                 single_round_offset += 360.0f;
             }
-            chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/60.0f;
+            chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/70.0f;
             break;
         case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-            chassis_cmd_recv.wz = 1.00;//实验版
+            chassis_cmd_recv.wz = -0.35;//实验版
             break;
         default:
             break;
     }
+
+    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
+    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东.)
     static float sin_theta, cos_theta;
-    // ------------------ 【修改部分：加入相位滞后动态补偿】 ------------------
+    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-    // 提取当前的云台-底盘夹角
+    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-    float real_offset_angle = chassis_cmd_recv.offset_angle;
+    // chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
-
+    // chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
-    // 如果处于小陀螺模式（或者 wz 不为 0），根据旋转速度进行超前补偿
+    chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;//实验版
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE )
+     chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
-    {
-        // 补偿系数 K 需要实车微调。
-        // 根据你的描述 wz=0.95 时偏差约45度，初始 K 值可以设为 45.0 / 0.95 ≈ 47.3f
-        float lag_compensation_K = 47.3f;//47.3f;45.0f
-
-        // 符号(+或-)取决于你的电机正方向、底盘旋转正方向，如果不正就改成减号
-        real_offset_angle -= (chassis_cmd_recv.wz * lag_compensation_K);
-        cos_theta = arm_cos_f32(real_offset_angle * DEGREE_2_RAD);
-        sin_theta = arm_sin_f32(real_offset_angle * DEGREE_2_RAD);
-
-    }else if (chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
-    {
-        cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-        sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-
-    }
-    // ----------------------------------------------------------------------
-        chassis_vx = (chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta)/4;
-        chassis_vy = (-chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta)/4;
-
-    // 使用补偿后的角度进行坐标系投影
-
-
-    // static float sin_theta, cos_theta;
-    // cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-    // sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-    // chassis_vx = (chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta)/4;
-    // chassis_vy = (-chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta)/4;
 
     // 根据控制模式进行正运动学解算,计算底盘输出
     SteeringWheelCalculate();
     // MecanumCalculate();
 
-
     // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
     LimitChassisOutput();
 
     // 根据电机的反馈速度和IMU(如果有)计算真实速度
     EstimateSpeed();
 
-    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送/
+    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
     // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
     // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
     // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况

application/cmd/robot_cmd.c

@@ -87,17 +87,9 @@ void RobotCMDInit()
 static void CalcOffsetAngle()
 {
     static float last_offset_angle = 0;
-    //const float smooth_factor = 0.3f; // 平滑因子,
+    const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显
 
-     float smooth_factor;
+    const float deadband = 3.0f;       // 死区值
-    // 小陀螺模式下使用更大的平滑因子，让offset_angle更快跟随云台
-    if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE) {
-        smooth_factor = 0.9f;  // 快速响应
-    } else {
-        smooth_factor = 0.3f;  // 平滑过渡 0~1之间,值越大平滑效果越明显
-    }
-
-    //const float deadband = 3.0f;       // 死区值
     // 新增：跟踪模式变化，用于重置基准角度
     // static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;//
     //
@@ -119,14 +111,14 @@ static void CalcOffsetAngle()
     else if (angle_diff < -180.0f)
         new_offset += 360.0f;
     /////
-   //  // 应用死区
+    // 应用死区
-   //  if (fabs(new_offset) < deadband) {
+    if (fabs(new_offset) < deadband) {
-   //      new_offset = 0.0f;
+        new_offset = 0.0f;
-   //  } else if (new_offset > 0) {
+    } else if (new_offset > 0) {
-   //     new_offset -= deadband;
+       new_offset -= deadband;
-   // } else {
+   } else {
-   //      new_offset += deadband;
+        new_offset += deadband;
-   //  }//////
+    }//////
          chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor;
      last_offset_angle = chassis_cmd_send.offset_angle;
     // // 更新模式状态
@@ -195,12 +187,12 @@ static void RemoteControlSet()
         // around = 0;
         chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
         gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-        around= chassis_cmd_send.offset_angle/(-360);
+        around= chassis_cmd_send.offset_angle/360;
     }
     else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
     {
         //gimbal_fetch_data.yaw_motor_total_angle = gimbal_fetch.yaw_motor_total_angle-around*360;
-        chassis_cmd_send.offset_angle =  chassis_cmd_send.offset_angle + 360*around;
+        chassis_cmd_send.offset_angle =  chassis_cmd_send.offset_angle - 360*around;
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
     }

application/gimbal/gimbal.c

@@ -85,12 +85,12 @@ void GimbalInit()
         //     },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 0.53, //0.55
+                .Kp = 0.75, //0.55
-                .Ki = 0.1,//0.05
+                .Ki = 0.05,//0.05
-                .Kd = 0.0148,//0.02
+                .Kd = 0.02,//0.02
                 .CoefA =0.5,//0.5
                 .CoefB = 0.6,//0.6
-                .Output_LPF_RC = 0.0068,
+                .Output_LPF_RC = 0.01,
                 .DeadBand = 0.005,//0.02
                 .Derivative_LPF_RC=0.01,//0.008
                 .Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement |  PID_OutputFilter |PID_DerivativeFilter,
@@ -98,15 +98,15 @@ void GimbalInit()
                 .MaxOut = 600,
             },
             .speed_PID = {
-                .Kp = 3400,//22000
+                .Kp = 500,//22000
-                .Ki = 15,//
+                .Ki = 0,//
-                .Kd =1,
+                .Kd =0.0005,
-                 .CoefA = 0.8,
+                // .CoefA = 0.8,
-                 .CoefB = 0.1,
+                // .CoefB = 0.1,
                 .Output_LPF_RC = 0.002,//0.002
                 .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement |  PID_OutputFilter,
-                .IntegralLimit = 6000,
+                .IntegralLimit = 5000,
-                .MaxOut = 30000,
+                .MaxOut = 20000,
             },
             .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
             // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明

application/robot_def.h

@@ -27,7 +27,7 @@
 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
 #define YAW_CHASSIS_ALIGN_ECD -3595  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
-#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
+#define YAW_ECD_GREATER_THAN_4096  0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
 #define PITCH_MAX_ANGLE 30           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
 #define PITCH_MIN_ANGLE -20           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
@@ -35,8 +35,8 @@
 // #define CHASSIS_OMNI_WHEEL // 是否为全向轮底盘
 // #define CHASSIS_MCNAMEE_WHEEL     // 是否为麦克纳姆轮底盘
 #define CHASSIS_STEERING_WHEEL               // 是否为舵轮
-#define STEERING_CHASSIS_ALIGN_ECD_RF   7066 // 舵电机 A 编码器值，若有机械改动需要修改
+#define STEERING_CHASSIS_ALIGN_ECD_RF   7330 // 舵电机 A 编码器值，若有机械改动需要修改
-#define STEERING_CHASSIS_ALIGN_ECD_LB  7138 // 舵电机 D 编码器值，若有机械改动需要修改
+#define STEERING_CHASSIS_ALIGN_ECD_LB  5099 // 舵电机 D 编码器值，若有机械改动需要修改
 
 #define STEERING_CHASSIS_ALIGN_ANGLE_RF STEERING_CHASSIS_ALIGN_ECD_RF / 8192.f * 360.f // 舵轮 A 对齐角度
 #define STEERING_CHASSIS_ALIGN_ANGLE_LB STEERING_CHASSIS_ALIGN_ECD_LB / 8192.f * 360.f // 舵轮 D 对齐角度

modules/remote/remote_control.c

@@ -40,7 +40,7 @@ static void sbus_to_rc(const uint8_t *sbus_buf)
     rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
     rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET;                       //!< Channel 3
     rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET;
-    rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+5.5;
+    rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+2.5;
     // 左侧拨轮
     RectifyRCjoystick();
     // 开关,0左1右