小陀螺家庭专业版

application/chassis/chassis.c

@@ -60,9 +60,6 @@ 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;
 
@@ -202,7 +199,6 @@ 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
 }
@@ -374,7 +370,6 @@ 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);
@@ -416,68 +411,17 @@ 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.95;//实验版
             break;
         default:
             break;
     }
 
-    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
-    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东.)
     static float sin_theta, cos_theta;
-    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;
-
-
-    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);
-    }
+    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;
-    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();
@@ -489,7 +433,7 @@ void ChassisTask() {
     // 根据电机的反馈速度和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,9 +87,17 @@ void RobotCMDInit()
 static void CalcOffsetAngle()
 {
     static float last_offset_angle = 0;
-    const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显
+    //const float smooth_factor = 0.3f; // 平滑因子,
 
-    const float deadband = 3.0f;       // 死区值
+     float smooth_factor;
+    // 小陀螺模式下使用更大的平滑因子，让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;//
     //
@@ -111,14 +119,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;
     // // 更新模式状态

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