方案——赛场_1.1

application/chassis/chassis.c

@@ -235,7 +235,7 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
     while (norm_rotation < -180.0f) {
         norm_rotation += 360.0f;
     }
-    float threshold = 110.0f;
+    float threshold = 90.0f;
     float target_single=fmodf(target_angle,360.0f);
     if(target_single<0.0f) target_single+=360.0f;
 

application/cmd/robot_cmd.c

@@ -45,6 +45,7 @@ static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息
 static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
 
 static Robot_Status_e robot_state; // 机器人整体工作状态
+static uint8_t robot_level = 1; // 机器人等级，范围1-10，上电重置为1
 
 void RobotCMDInit()
 {
@@ -189,7 +190,7 @@ static void RemoteControlSet()
     }
     else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
     {
-        //gimbal_fetch_data.yaw_motor_total_angle = gimbal_fetch_data.yaw_motor_total_angle-around*360;
+        //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.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
@@ -214,8 +215,8 @@ static void RemoteControlSet()
         gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
 
     // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
-    chassis_cmd_send.vx = -(0.15f * (float)rc_data[TEMP].rc.rocker_r_); // _水平方向
+    chassis_cmd_send.vx = (0.25f * (float)rc_data[TEMP].rc.rocker_r_); // _水平方向
-    chassis_cmd_send.vy = -(0.15f * (float)rc_data[TEMP].rc.rocker_r1); // 1数值方向
+    chassis_cmd_send.vy = (0.25f * (float)rc_data[TEMP].rc.rocker_r1); // 1数值方向 .
 
     // // 发射参数
     // if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
@@ -243,10 +244,53 @@ static void RemoteControlSet()
  */
 static void MouseKeySet()
 {
+
+    // // G键控制机器人等级，每次按G键等级+1，最高10级
+    // if (rc_data[TEMP].key[KEY_PRESS].g && rc_data[TEMP].key_count[KEY_PRESS][Key_G] > 0) {
+    //     robot_level = (robot_level % 10) + 1; // 等级循环增加，1-10
+    // }
+    // G键控制机器人等级，每次按G键等级+1，最高10级
+    if (rc_data[TEMP].key[KEY_PRESS].g && rc_data[TEMP].key_count[KEY_PRESS][Key_G] > 0) {
+        robot_level = (robot_level % 10) + 1; // 等级循环增加，1-10
+        rc_data[TEMP].key_count[KEY_PRESS][Key_G] = 0; // 清除计数器，防止重复执行
+    }
+
+    switch (robot_level)
+    {
+        case 1:
+        case 2:
+        case 3:
+            chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 20 - rc_data[TEMP].key[KEY_PRESS].d * 20); // w向前，s向后
+            chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 20 - rc_data[TEMP].key[KEY_PRESS].s * 20); // a向左，d向右
+            break;
+        case 4:
+        case 5:
+        case 6:
+        case 7:
+            chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 30 - rc_data[TEMP].key[KEY_PRESS].d * 30); // w向前，s向后
+            chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 30 - rc_data[TEMP].key[KEY_PRESS].s * 30); // a向左，d向右
+            break;
+        default:
+            chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 40 - rc_data[TEMP].key[KEY_PRESS].d * 40); // w向前，s向后
+            chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 40 - rc_data[TEMP].key[KEY_PRESS].s * 40); // a向左，d向右
+            break;
+    }
+    // 根据等级设置底盘速度
+    // if (robot_level >= 1 && robot_level <= 3) {
+    //     chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 20 - rc_data[TEMP].key[KEY_PRESS].d * 20); // w向前，s向后
+    //     chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 20 - rc_data[TEMP].key[KEY_PRESS].s * 20); // a向左，d向右
+    // } else if (robot_level >= 4 && robot_level <= 7) {
+    //     chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 30 - rc_data[TEMP].key[KEY_PRESS].d * 30); // w向前，s向后
+    //     chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 30 - rc_data[TEMP].key[KEY_PRESS].s * 30); // a向左，d向右
+    // } else {
+    //     chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 40 - rc_data[TEMP].key[KEY_PRESS].d * 40); // w向前，s向后
+    //     chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 40 - rc_data[TEMP].key[KEY_PRESS].s * 40); // a向左，d向右
+    // }
+
     // chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
     // chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
-    chassis_cmd_send.vx = -(rc_data[TEMP].key[KEY_PRESS].a * 27 - rc_data[TEMP].key[KEY_PRESS].d * 27); // w向前，s向后
+    //chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 27 - rc_data[TEMP].key[KEY_PRESS].d * 27); // w向前，s向后
-    chassis_cmd_send.vy = -(rc_data[TEMP].key[KEY_PRESS].w * 27 - rc_data[TEMP].key[KEY_PRESS].s * 27); // a向左，d向右
+    //chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 27 - rc_data[TEMP].key[KEY_PRESS].s * 27); // a向左，d向右
 
     gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
     gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 8;

application/gimbal/gimbal.c

@@ -15,7 +15,7 @@ static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
 static float last_yaw_angle = 0.0f;   // 保护模式前的yaw角度
-// static float last_pitch_angle = 0.0f; // 保护模式前的pitch角度
+ static float last_pitch_angle = 0.0f; // 保护模式前的pitch角度
 uint8_t in_protection_mode = 0; // 是否在保护模式
 void GimbalInit()
 {
@@ -148,7 +148,7 @@ void GimbalTask()
         // 进入保护模式时记录当前角度
         if (!in_protection_mode) {
             last_yaw_angle = gimba_IMU_data->YawTotalAngle;
-            // last_pitch_angle = gimba_IMU_data->Pitch;
+             last_pitch_angle = gimba_IMU_data->Pitch;
             in_protection_mode = 1;
         }
         DJIMotorStop(yaw_motor);

application/robot_def.h

@@ -26,7 +26,7 @@
 
 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
-#define YAW_CHASSIS_ALIGN_ECD 510  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
+#define YAW_CHASSIS_ALIGN_ECD -3686  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096  0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
 #define PITCH_MAX_ANGLE 30           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
@@ -208,7 +208,8 @@ typedef struct
 {
     attitude_t gimbal_imu_data;
     uint16_t yaw_motor_single_round_angle;
-    uint16_t yaw_motor_total_angle;
+    //uint16_t yaw_motor_total_angle;
+    float yaw_motor_total_angle;
 } Gimbal_Upload_Data_s;
 
 typedef struct