小陀螺前瞻版

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
@@ -60,6 +60,9 @@ static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据
 static SuperCapInstance *cap;                                       // 超级电容
 static LKMotorInstance *motor_rf,*motor_lb;                         // 轮电机9015
 static DJIMotorInstance *motor_steering_rf,*motor_steering_lb;     // 舵电机6020
+static Gimbal_Upload_Data_s gimbal_feed_data;                      // 云台反馈数据
+static float last_imu_yaw = 0.0f;                                  // IMU角度上一帧值
+static Subscriber_t *gimbal_feed_sub;  // 新增,用于订阅云台的反馈数据
 /* 用于自旋变速策略的时间变量 */
 // static float t;
 
@@ -157,12 +160,12 @@ void ChassisInit()
         .Ki                = 0,
         .Kd                = 0.1f, // 0.5
         .DeadBand          = 0.5,
-        .CoefA             = 0.2,
-        .CoefB             = 0.3,
-        .Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
-        .IntegralLimit     = 50, // 200
-        .MaxOut            = 1200,
-        .Derivative_LPF_RC = 0.01, // 0.01
+        // .CoefA             = 0.2,
+        // .CoefB             = 0.3,
+        // .Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
+        // .IntegralLimit     = 50, // 200
+        .MaxOut            = 50,
+        // .Derivative_LPF_RC = 0.01, // 0.01
     };
     PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf);
 
@@ -199,6 +202,7 @@ void ChassisInit()
 
 #ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息
     chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
+    gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));  // 新增
     chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
 #endif // ONE_BOARD
 }
@@ -370,6 +374,7 @@ void ChassisTask() {
     // 获取新的控制信息
 #ifdef ONE_BOARD
     SubGetMessage(chassis_sub, &chassis_cmd_recv);
+    SubGetMessage(gimbal_feed_sub, &gimbal_feed_data);
 #endif
 #ifdef CHASSIS_BOARD
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
@@ -411,7 +416,7 @@ void ChassisTask() {
             chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/70.0f;
             break;
         case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-            chassis_cmd_recv.wz = -0.35;//实验版
+            chassis_cmd_recv.wz = 0.35;//实验版
             break;
         default:
             break;
@@ -420,12 +425,59 @@ void ChassisTask() {
     // 根据云台和底盘的角度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);
+    static float rotate_offset_angle = 0.0f;  // 小陀螺模式下的旋转角度补偿
+    static float offset_angle_base = 0.0f;  // 进入小陀螺模式时的offset_angle基准
+    static uint8_t last_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;
     // chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
-    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;
+
+
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) {
+        // 小陀螺模式：使用角速度积分来计算旋转补偿
+        if (last_chassis_mode != CHASSIS_ROTATE) {
+            // 刚进入小陀螺模式时，初始化补偿角度/
+            rotate_offset_angle = 0.0f;
+            offset_angle_base = chassis_cmd_recv.offset_angle;
+            last_imu_yaw = gimbal_feed_data.gimbal_imu_data.YawTotalAngle;  // 添加这行,初始化IMU角度
+
+        }
+        float offset_angle_delta = chassis_cmd_recv.offset_angle - offset_angle_base;
+        // 使用角速度积分（累加），而不是角速度变化量
+        //rotate_offset_angle -= chassis_cmd_recv.wz * 0.005f * RAD_2_DEGREE;
+
+        // 使用IMU实际角度变化补偿
+        float current_imu_yaw = gimbal_feed_data.gimbal_imu_data.YawTotalAngle;
+        float imu_delta = current_imu_yaw - last_imu_yaw;
+        if (imu_delta > 180.0f) imu_delta -= 360.0f;
+        else if (imu_delta < -180.0f) imu_delta += 360.0f;
+        rotate_offset_angle += imu_delta;
+        last_imu_yaw = current_imu_yaw;
+
+        // 限制补偿角度范围，防止积分饱和
+        if (rotate_offset_angle > 180.0f) rotate_offset_angle -= 360.0f;
+        if (rotate_offset_angle < -180.0f) rotate_offset_angle += 360.0f;
+
+        // 使用补偿后的角度进行坐标变换
+        //float effective_angle = chassis_cmd_recv.offset_angle - rotate_offset_angle;/
+        float effective_angle = offset_angle_delta - rotate_offset_angle;
+        cos_theta = arm_cos_f32(effective_angle * DEGREE_2_RAD);
+        sin_theta = arm_sin_f32(effective_angle * DEGREE_2_RAD);
+    } else {
+        // 非小陀螺模式：正常使用offset_angle
+        rotate_offset_angle = 0.0f;  // 重置补偿角度
+        offset_angle_base = chassis_cmd_recv.offset_angle;  // 添加这行,更新基准角度
+        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);
+    }
+
+    last_chassis_mode = chassis_cmd_recv.chassis_mode;
+
+    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();

application/cmd/robot_cmd.c

@@ -187,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;
     }

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_+2.5;
+    rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+0.5;
     // 左侧拨轮
     RectifyRCjoystick();
     // 开关,0左1右