fix speed

User/component/vmc.c

@@ -122,7 +122,11 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
     VMC_ANGLE_NORMALIZE(leg->phi0);
     
     // 计算角速度和长度变化率
-    leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
+    float dphi0 = leg->phi0 - leg->last_phi0;
+    /* phi0 在 ±π 跳变时做循环差值处理 */
+    if (dphi0 > VMC_PI)  dphi0 -= VMC_2PI;
+    if (dphi0 < -VMC_PI) dphi0 += VMC_2PI;
+    leg->d_phi0 = (vmc->dt > 0) ? (dphi0 / vmc->dt) : 0.0f;
     leg->d_alpha = -leg->d_phi0;
     leg->d_theta = body_pitch_rate + leg->d_phi0;
     leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);

User/device/referee.c

@@ -649,6 +649,26 @@ uint8_t Referee_UIRefresh(Referee_UI_t *ui) {
                     ui->screen->width * 0.4, ui->screen->height * 0.2,
                     ui->screen->width * 0.4f + sinf(ui->chassis_ui.angle) * 46.0f,
                     ui->screen->height * 0.2f + cosf(ui->chassis_ui.angle) * 46.0f);
+        UI_DrawLine(Referee_GetGrapicAdd(ui), "7", UI_GRAPIC_OPERATION_ADD,
+                    UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12,
+                    ui->screen->width * 0.4, ui->screen->height * 0.4,
+                    ui->screen->width * 0.4f - sinf(ui->chassis_ui.l0[0]) * 46.0f,
+                    ui->screen->height * 0.4f - cosf(ui->chassis_ui.l0[0]) * 46.0f);       
+        UI_DrawLine(Referee_GetGrapicAdd(ui), "8", UI_GRAPIC_OPERATION_ADD,
+                    UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12,
+                    ui->screen->width * 0.4, ui->screen->height * 0.6,
+                    ui->screen->width * 0.4f - sinf(ui->chassis_ui.theta[0]) * 46.0f,
+                    ui->screen->height * 0.6f - cosf(ui->chassis_ui.theta[0]) * 46.0f);   
+        UI_DrawLine(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD,
+                    UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12,
+                    ui->screen->width * 0.4, ui->screen->height * 0.4,
+                    ui->screen->width * 0.4f - sinf(ui->chassis_ui.l0[1]) * 46.0f,
+                    ui->screen->height * 0.4f - cosf(ui->chassis_ui.l0[1]) * 46.0f);
+        UI_DrawLine(Referee_GetGrapicAdd(ui), "a", UI_GRAPIC_OPERATION_ADD,
+                    UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12,
+                    ui->screen->width * 0.4, ui->screen->height * 0.8,
+                    ui->screen->width * 0.4f - sinf(ui->chassis_ui.theta[1]) * 46.0f,
+                    ui->screen->height * 0.8f - cosf(ui->chassis_ui.theta[1]) * 46.0f);                                  
         float start_pos_h = 0.0f;
         switch (ui->chassis_ui.mode) {
           case CHASSIS_MODE_RELAX:

User/device/referee_proto_types.h

@@ -59,6 +59,7 @@ typedef struct {
   Referee_Status_t ref_status;
   float chassis_power_limit;
   float chassis_pwr_buff;
+ 
 } Referee_ForChassis_t;
 
 typedef struct {

User/module/aimbot.c

@@ -93,7 +93,10 @@ int8_t Aimbot_Init(Aimbot_Param_t *param) {
   /* 注册 AI 指令帧队列（下层板接收／上层板发送） */
   BSP_FDCAN_RegisterId(param->can, param->cmd_id,
                      BSP_FDCAN_DEFAULT_QUEUE_SIZE);
-
+ for (uint8_t i = 0; i < 3; i++) {
+    BSP_CAN_RegisterId(param->can, param->cmd_id + i,
+                       10);
+  }
   /* 注册反馈数据帧队列（上层板接收／下层板发送） */
   for (uint8_t i = 0; i < AIMBOT_FB_FRAME_NUM; i++) {
     BSP_FDCAN_RegisterId(param->can, param->fb_base_id + i,

User/module/balance_chassis.c

@@ -34,6 +34,7 @@ static void Chassis_SelectYawZeroPoint(Chassis_t *c);
 static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
 static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
 static void Chassis_RecoverControl(Chassis_t *c);
+static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
 
 /* Private functions -------------------------------------------------------- */
 
@@ -85,6 +86,8 @@ static void Chassis_ResetControllers(Chassis_t *c) {
   PID_Reset(&c->pid.roll_force);
   PID_Reset(&c->pid.tp[0]);
   PID_Reset(&c->pid.tp[1]);
+  PID_Reset(&c->pid.leg_test_theta[0]);
+  PID_Reset(&c->pid.leg_test_theta[1]);
 
   /* 重置LQR控制器 */
   LQR_Reset(&c->lqr[0]);
@@ -209,6 +212,11 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
     c->climb.state_start_time = 0;
   }
 
+  /* 进入腿部测试模式：重置初始化标志，进入后锁定当前摆角 */
+  if (mode == CHASSIS_MODE_LEG_TEST) {
+    c->leg_test.initialized = false;
+  }
+
   c->mode = mode;
   return CHASSIS_OK;
 }
@@ -665,6 +673,88 @@ static void Chassis_RecoverControl(Chassis_t *c) {
 }
 
 
+/**
+ * @brief 腿部测试控制
+ * @param c 底盘结构体指针
+ * @param c_cmd 控制指令
+ * @note 进入时锁定当前摆角，vx/vy 增减左/右腿目标摆角
+ *       PID控制摆角，恒力支撑腿长，轮子关闭
+ */
+static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
+  /* 首次进入：锁定当前摆角 */
+  if (!c->leg_test.initialized) {
+    c->leg_test.target_theta[0] = c->vmc_[0].leg.theta;
+    c->leg_test.target_theta[1] = c->vmc_[1].leg.theta;
+    c->leg_test.initialized = true;
+  }
+
+  float sens = c->param->leg_test.theta_sensitivity;
+  float deadzone = 0.05f;  /* 摇杆死区 */
+
+  /* vx 控制左腿：有输入→累加目标角度（正/反转），松手→锁住当前实际角度 */
+  if (fabsf(c_cmd->move_vec.vx) > deadzone) {
+    c->leg_test.target_theta[0] += c_cmd->move_vec.vx * sens * c->dt;
+  } else {
+    c->leg_test.target_theta[0] = c->vmc_[0].leg.theta;
+  }
+
+  /* vy 控制右腿：同理 */
+  if (fabsf(c_cmd->move_vec.vy) > deadzone) {
+    c->leg_test.target_theta[1] += c_cmd->move_vec.vy * sens * c->dt;
+  } else {
+    c->leg_test.target_theta[1] = c->vmc_[1].leg.theta;
+  }
+
+  /* 目标角度归一化到 [-π, π]，防止无限增长 */
+  while (c->leg_test.target_theta[0] > (float)M_PI)  c->leg_test.target_theta[0] -= (float)M_2PI;
+  while (c->leg_test.target_theta[0] < -(float)M_PI) c->leg_test.target_theta[0] += (float)M_2PI;
+  while (c->leg_test.target_theta[1] > (float)M_PI)  c->leg_test.target_theta[1] -= (float)M_2PI;
+  while (c->leg_test.target_theta[1] < -(float)M_PI) c->leg_test.target_theta[1] += (float)M_2PI;
+
+  /* 轮子关闭：只清零输出，由 Chassis_Output 统一发送，避免重复 CAN 帧 */
+
+  /* 恒定支撑力 + PID摆角控制 → VMC逆解算 → 关节力矩 */
+  float support_force = c->param->leg_test.support_force;
+
+  for (int i = 0; i < 2; i++) {
+    /* 手动计算循环误差，避免 theta 在 ±π 跳变时 PID 炸 */
+    float error = c->leg_test.target_theta[i] - c->vmc_[i].leg.theta;
+    while (error > (float)M_PI)  error -= (float)M_2PI;
+    while (error < -(float)M_PI) error += (float)M_2PI;
+
+    /* 用误差直接算 P 和 D 输出，绕过 PID 内部的 set/fdb 差值 */
+    float kp = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->p;
+    float kd = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->d;
+    float out_limit = c->pid.leg_test_theta[i].param->out_limit;
+    float Tp = kp * error - kd * c->vmc_[i].leg.d_theta;
+    Tp = LIMIT(Tp, -out_limit, out_limit);
+
+    if (VMC_InverseSolve(&c->vmc_[i], support_force, Tp) == 0) {
+      if (i == 0) {
+        VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[0], &c->output.joint[1]);
+      } else {
+        VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[3], &c->output.joint[2]);
+      }
+    } else {
+      c->output.joint[i * 2] = 0.0f;
+      c->output.joint[i * 2 + 1] = 0.0f;
+    }
+  }
+
+  /* 轮子输出清零 */
+  c->output.wheel[0] = 0.0f;
+  c->output.wheel[1] = 0.0f;
+
+  /* 安全限制 */
+  for (int i = 0; i < 4; i++) {
+    c->output.joint[i] = LIMIT(c->output.joint[i], -30.0f, 30.0f);
+  }
+
+  /* 发送关节指令 */
+  Chassis_Output(c);
+}
+
+
 /**
  * @brief 初始化底盘
  * @param c 底盘结构体指针
@@ -713,6 +803,8 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force);
   PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
   PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
+  PID_Init(&c->pid.leg_test_theta[0], KPID_MODE_CALC_D, target_freq, &param->leg_test_theta);
+  PID_Init(&c->pid.leg_test_theta[1], KPID_MODE_CALC_D, target_freq, &param->leg_test_theta);
 
   /* 初始化LQR控制器 */
   LQR_Init(&c->lqr[0], &param->lqr_gains);
@@ -814,6 +906,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   c->recover.state[1] = RECOVER_STRETCH;
   c->recover.state_start_time = 0;
 
+  /* 初始化腿部测试状态 */
+  c->leg_test.initialized = false;
+  c->leg_test.target_theta[0] = 0.0f;
+  c->leg_test.target_theta[1] = 0.0f;
+
   return CHASSIS_OK;
 }
 
@@ -980,6 +1077,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       Chassis_ClimbControl(c, c_cmd);
       break;
 
+    case CHASSIS_MODE_LEG_TEST:
+      Chassis_LegTestControl(c, c_cmd);
+      break;
+
     default:
       return CHASSIS_ERR_MODE;
   }
@@ -1018,7 +1119,7 @@ void Chassis_Output(Chassis_t *c) {
   //   c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
   // }
 	//cap-add//supercap-add
-	PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
+	// PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
 	//PowerLimit_ChassicOutput()
   MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
   MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
@@ -1042,7 +1143,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     c->chassis_state.target_velocity_x = -world_vx * sinf(gimbal_offset) + world_vy * cosf(gimbal_offset);
     c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
   } else {
-    c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 3.0f;
+    c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
     c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
   }
 
@@ -1375,6 +1476,9 @@ int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  */
 void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui) {
   ui->mode = c->mode;
-  // ui->angle = c->feedback.yaw.rotor_abs_angle - c->mech_zero;
+  ui->l0[0] = c->vmc_[0].leg.L0;
+  ui->l0[1] = c->vmc_[1].leg.L0;
+  ui->theta[0] = c->vmc_[0].leg.theta;
+  ui->theta[1] = c->vmc_[1].leg.theta;
   // #error "右边那个mech_zero应该是跟随云台的那个角,我没找着在哪"
 }

User/module/balance_chassis.h

@@ -43,7 +43,8 @@ typedef enum {
   // CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
   CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
   CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/
-  CHASSIS_MODE_CLIMB_STEP  /* 上台阶模式 */
+  CHASSIS_MODE_CLIMB_STEP,  /* 上台阶模式 */
+  CHASSIS_MODE_LEG_TEST     /* 腿部测试模式：PID锁摆角+恒力支撑，摇杆控制摆角 */
 } Chassis_Mode_t;
 
 /* 跳跃状态枚举 */
@@ -101,6 +102,9 @@ typedef struct {
 typedef struct {
   Chassis_Mode_t mode;
   float angle;
+  /* 轮腿角度和距轮长 */
+  float l0[2];
+  float theta[2];
 } Chassis_RefereeUI_t;
 
 /* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
@@ -174,6 +178,14 @@ typedef struct {
     uint32_t settle_time_ms;       /* 收腿后稳定时间 (ms) */
   } climb;
 
+  /* 腿部测试参数 */
+  struct {
+    float support_force;           /* 恒定支撑力 (N) */
+    float theta_sensitivity;       /* 摇杆→摆角灵敏度 (rad/s) */
+  } leg_test;
+
+  KPID_Params_t leg_test_theta;    /* 腿部测试摆角PID（独立于tp） */
+
   /* 倒地自起参数 */
   struct {
     float stretch_velocity;        /* 伸腿角速度 (rad/s) */
@@ -267,6 +279,12 @@ typedef struct {
     uint32_t state_start_time;   /* 当前状态开始时间 (ms) */
   } climb;
 
+  /* 腿部测试相关 */
+  struct {
+    float target_theta[2];       /* 左右腿目标摆角 (rad)，进入时锁定当前值 */
+    bool initialized;            /* 是否已锁定初始摆角 */
+  } leg_test;
+
   /* 电机离线检测 */
   struct {
     bool any_offline;        /* 是否有电机离线 */
@@ -286,6 +304,7 @@ typedef struct {
     KPID_t tp[2];
     KPID_t leg_length[2]; /* 两条腿的腿长PID */
     KPID_t leg_theta[2]; /* 两条腿的摆角PID */
+    KPID_t leg_test_theta[2]; /* 腿部测试摆角PID */
   } pid;
 
   /* 滤波器 */

User/module/cmd/cmd.c

@@ -3,6 +3,8 @@
  */
 #include "cmd.h"
 #include "bsp/time.h"
+#include "module/cmd/cmd_types.h"
+#include "module/shoot.h"
 #include <stdint.h>
 #include <string.h>
 
@@ -68,7 +70,7 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
 #if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT
 static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
   if (ctx->input.online[CMD_SRC_RC]) {
-      ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST;
+      ctx->output.shoot.cmd.mode = SHOOT_MODE_CONTINUE;
   } else {
       ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
   }
@@ -173,9 +175,6 @@ static void CMD_PC_BuildShootCmd(CMD_t *ctx) {
   
   ctx->output.shoot.cmd.ready = true;
   ctx->output.shoot.cmd.firecmd = ctx->input.pc.mouse.l_click;
-
-  CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_SHOOT);
-
 }
 #endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT */
 
@@ -208,11 +207,16 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
   
   /* 使用AI提供的云台控制数据 */
   
-  if (ctx->input.nuc.mode!=0) {
+  // if (ctx->input.nuc.mode!=0) {
   ctx->output.gimbal.cmd.mode = GIMBAL_MODE_AI_CONTROL;
+  ctx->output.gimbal.cmd.mode_ai = (uint8_t)ctx->input.nuc.mode; /* 直接使用AI模式细分字段 */
   ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
   ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
-  }
+  ctx->output.gimbal.cmd.ff_vel_yaw = ctx->input.nuc.gimbal.vel.yaw;
+  ctx->output.gimbal.cmd.ff_vel_pit = ctx->input.nuc.gimbal.vel.pit;
+  ctx->output.gimbal.cmd.ff_accl_yaw = ctx->input.nuc.gimbal.accl.yaw;
+  ctx->output.gimbal.cmd.ff_accl_pit = ctx->input.nuc.gimbal.accl.pit;
+  // }
 
 }
 #endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL */
@@ -228,8 +232,9 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
   /* 根据AI模式决定射击行为 */
   switch (ctx->input.nuc.mode) {
     case 0:
-    case 1:  
+    case 1:
-      ctx->output.shoot.cmd.ready = true;
+      /* mode=1 只控云台，不让射击机构进入 ready，避免摩擦轮继续预转。 */
+      ctx->output.shoot.cmd.ready = false;
       ctx->output.shoot.cmd.firecmd = false; 
       break;
     case 2:
@@ -242,6 +247,10 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
         ctx->output.shoot.cmd.firecmd = false; 
       }
       break;
+    default:
+      ctx->output.shoot.cmd.ready = false;
+      ctx->output.shoot.cmd.firecmd = false;
+      break;
   }
 }
 #endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT */
@@ -372,6 +381,7 @@ static void CMD_RC_BuildBalanceChassisCmd(CMD_t *ctx) {
   /* 拨轮传递目标高度 */
   ctx->output.balance_chassis.cmd.height       = ctx->input.rc.dial;
   ctx->output.balance_chassis.cmd.jump_trigger  = false;
+
 }
 #endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_BALANCE_CHASSIS */
 
@@ -385,13 +395,12 @@ static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) {
     return;
   }
 
-  ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+  ctx->output.balance_chassis.cmd.move_vec.vx = 0.0f;
-
-  float vx = 0.0f, wz = 0.0f;
-
-  ctx->output.balance_chassis.cmd.move_vec.vx = vx;
-  ctx->output.balance_chassis.cmd.move_vec.wz = wz;
   ctx->output.balance_chassis.cmd.move_vec.vy = 0.0f;
+  ctx->output.balance_chassis.cmd.move_vec.wz = 0.0f;
+
+  ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+  CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_BALANCE_CHASSIS);
 
 }
 #endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_BALANCE_CHASSIS */
@@ -635,19 +644,19 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
 
   CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE);
 
-// #if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
+#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
-//   if (ctx->input.online[CMD_SRC_NUC]) {
+  if (ctx->input.online[CMD_SRC_NUC]) {
-//     if (ctx->active_source==CMD_SRC_RC) {
+    if (ctx->active_source==CMD_SRC_RC) {
-//       if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
+      if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
-//         ctx->output.gimbal.source = CMD_SRC_NUC;
+        ctx->output.gimbal.source = CMD_SRC_NUC;
-//         ctx->output.shoot.source = CMD_SRC_NUC;
+        ctx->output.shoot.source = CMD_SRC_NUC;
 #if CMD_ENABLE_MODULE_REFUI
         ctx->output.refui.source = CMD_SRC_NUC;
 #endif
-//       }
+      }
-//     }
+    }
-//   }
+  }
-// #endif
+#endif
 
   return CMD_OK;
 }

User/module/cmd/cmd_behavior.c

@@ -19,7 +19,7 @@ int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) {
     ctx->output.chassis.cmd.ctrl_vec.vy += ctx->config->sensitivity.move_sens;
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
-    ctx->output.balance_chassis.cmd.move_vec.vy += ctx->config->sensitivity.move_sens;
+    ctx->output.balance_chassis.cmd.move_vec.vx = ctx->config->sensitivity.move_sens;
 #endif
     return CMD_OK;
 }
@@ -29,7 +29,7 @@ int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) {
     ctx->output.chassis.cmd.ctrl_vec.vy -= ctx->config->sensitivity.move_sens;
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
-    ctx->output.balance_chassis.cmd.move_vec.vy -= ctx->config->sensitivity.move_sens;
+    ctx->output.balance_chassis.cmd.move_vec.vx = -ctx->config->sensitivity.move_sens;
 #endif
     return CMD_OK;
 }
@@ -39,7 +39,7 @@ int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) {
     ctx->output.chassis.cmd.ctrl_vec.vx -= ctx->config->sensitivity.move_sens;
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
-    ctx->output.balance_chassis.cmd.move_vec.vx -= ctx->config->sensitivity.move_sens;
+    ctx->output.balance_chassis.cmd.move_vec.vy = -ctx->config->sensitivity.move_sens;
 #endif
     return CMD_OK;
 }
@@ -49,7 +49,7 @@ int8_t CMD_Behavior_Handle_RIGHT(CMD_t *ctx) {
     ctx->output.chassis.cmd.ctrl_vec.vx += ctx->config->sensitivity.move_sens;
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
-    ctx->output.balance_chassis.cmd.move_vec.vx += ctx->config->sensitivity.move_sens;
+    ctx->output.balance_chassis.cmd.move_vec.vy = ctx->config->sensitivity.move_sens;
 #endif
     return CMD_OK;
 }
@@ -132,6 +132,9 @@ int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) {
 #endif
 #if CMD_ENABLE_MODULE_ARM
       ctx->output.arm.source = CMD_SRC_RC;
+#endif
+#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
+      ctx->output.balance_chassis.source = CMD_SRC_RC;
 #endif
     } else if(ctx->active_source == CMD_SRC_RC) {
       ctx->active_source = CMD_SRC_PC;
@@ -146,6 +149,9 @@ int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) {
 #endif
 #if CMD_ENABLE_MODULE_ARM
       ctx->output.arm.source = CMD_SRC_PC;
+#endif
+#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
+      ctx->output.balance_chassis.source = CMD_SRC_PC;
 #endif
     }
     return CMD_OK;

User/module/cmd/cmd_types.h

@@ -254,12 +254,12 @@ typedef enum {
 /* 行为-按键映射宏表 */
 #define BEHAVIOR_CONFIG_COUNT (12)
 #define CMD_BEHAVIOR_TABLE(X) \
-  X(FORE,           CMD_KEY_W,     CMD_ACTIVE_PRESSED,      CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \
+  X(FORE,           CMD_KEY_W,     CMD_ACTIVE_PRESSED,      CMD_MODULE_BALANCE_CHASSIS) \
-  X(BACK,           CMD_KEY_S,     CMD_ACTIVE_PRESSED,      CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \
+  X(BACK,           CMD_KEY_S,     CMD_ACTIVE_PRESSED,      CMD_MODULE_BALANCE_CHASSIS) \
-  X(LEFT,           CMD_KEY_A,     CMD_ACTIVE_PRESSED,      CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \
+  X(LEFT,           CMD_KEY_A,     CMD_ACTIVE_PRESSED,      CMD_MODULE_BALANCE_CHASSIS) \
-  X(RIGHT,          CMD_KEY_D,     CMD_ACTIVE_PRESSED,      CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \
+  X(RIGHT,          CMD_KEY_D,     CMD_ACTIVE_PRESSED,      CMD_MODULE_BALANCE_CHASSIS) \
-  X(ACCELERATE,     CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED,      CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \
+  X(ACCELERATE,     CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED,      CMD_MODULE_BALANCE_CHASSIS) \
-  X(ROTOR,          CMD_KEY_CTRL,  CMD_ACTIVE_PRESSED,      CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \
+  X(ROTOR,          CMD_KEY_CTRL,  CMD_ACTIVE_PRESSED,      CMD_MODULE_BALANCE_CHASSIS) \
   X(FIRE,           CMD_KEY_L_CLICK,  CMD_ACTIVE_PRESSED,   CMD_MODULE_SHOOT)   \
   X(FIRE_MODE,      CMD_KEY_B,     CMD_ACTIVE_RISING_EDGE,  CMD_MODULE_SHOOT)   \
   X(CLIMB,          CMD_KEY_R,     CMD_ACTIVE_PRESSED,      CMD_MODULE_BALANCE_CHASSIS) \

User/module/config.c

@@ -140,7 +140,7 @@ Config_RobotParam_t robot_config = {
           .fric_num=2,
           .extra_deceleration_ratio=1.0f,
           .num_trig_tooth=8,
-          .shot_freq=1.0f,
+          .shot_freq=15.0f,
           .shot_burst_num=3,
           .ratio_multilevel = {1.0f},
         },   
@@ -152,7 +152,7 @@ Config_RobotParam_t robot_config = {
         .heatControl={
             .enable=true,
             .safe_shots=5,    // 安全出弹余量
-            .nmax=18.0f,      // 最大射频 Hz
+            .nmax=15.0f,      // 最大射频 Hz
             .Hwarn=200.0f,    // 热量预警值
             .Hsatu=100.0f,    // 热量饱和值
             .Hthres=50.0f,    // 热量阈值
@@ -491,6 +491,20 @@ Config_RobotParam_t robot_config = {
         .phi_tolerance = 0.5f,         /* 体姿接近站立容差 (rad)，约28° */
         .kd = 3.0f,                    /* MIT速度模式阻尼 */
     },
+    .leg_test = {
+        .support_force = 30.0f,        /* 恒定支撑力 (N) */
+        .theta_sensitivity = 1.0f,     /* 摇杆→摆角灵敏度 (rad/s) */
+    },
+    .leg_test_theta = {
+        .k = 20.0f,
+        .p = 5.0f,
+        .i = 0.0f,
+        .d = 1.0f,
+        .i_limit = 0.0f,
+        .out_limit = 30.0f,            /* 摆角力矩限幅 (Nm) */
+        .d_cutoff_freq = 30.0f,
+        .range = M_2PI,                /* 启用循环误差处理 */
+    },
     .lqr_offset = {
         .theta = 0.0f,
         .x = 0.0f,
@@ -515,21 +529,22 @@ Config_RobotParam_t robot_config = {
         },
         .sensitivity = {
             .mouse_sens     = 60.0f,
-            .move_sens      = 1.0f,
+            .move_sens      = 0.99f,
             .move_fast_mult = 2.0f,
             .move_slow_mult = 0.4f,
         },
         .rc_mode_map = {
 #if CMD_ENABLE_MODULE_GIMBAL
             .gimbal_sw_up   = GIMBAL_MODE_RELAX,
-            // .gimbal_sw_mid  = GIMBAL_MODE_ABSOLUTE,
+            .gimbal_sw_mid  = GIMBAL_MODE_ABSOLUTE,
-            .gimbal_sw_mid = GIMBAL_MODE_RECOVER,
+            // .gimbal_sw_down = GIMBAL_MODE_RECOVER,
-            .gimbal_sw_down = GIMBAL_MODE_RELATIVE,
+            .gimbal_sw_down = GIMBAL_MODE_AI_CONTROL,
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
             .balance_sw_up   = CHASSIS_MODE_RELAX,
-            // .balance_sw_mid  = CHASSIS_MODE_WHELL_LEG_BALANCE,
+            //  .balance_sw_mid   = CHASSIS_MODE_RELAX,
-            .balance_sw_mid = CHASSIS_MODE_RELAX, // 先关闭轮腿平衡，避免不熟悉操作导致摔倒
+            //   .balance_sw_down   = CHASSIS_MODE_RELAX,
+            .balance_sw_mid  =CHASSIS_MODE_WHELL_LEG_BALANCE,
             .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
 #endif
         },

User/module/gimbal.c

@@ -99,13 +99,13 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
   PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
            &(g->param->pid.pit_omega));
   PID_Init(&(g->pid.aimbot.yaw_angle), KPID_MODE_SET_D, target_freq,
-           &(g->param->pid.aimbot.yaw_angle));
+           &(g->param->pid.yaw_angle));
   PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq,
-           &(g->param->pid.aimbot.yaw_omega));
+           &(g->param->pid.yaw_omega));
   PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq,
-           &(g->param->pid.aimbot.pit_angle));
+           &(g->param->pid.pit_angle));
   PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq,
-           &(g->param->pid.aimbot.pit_omega));
+           &(g->param->pid.pit_omega));
 
   /* Recover模式PID（单环，编码器反馈） */
   PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq,
@@ -193,6 +193,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
   g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
   g->lask_wakeup = BSP_TIME_Get_us();
 
+  /* AI自瞄保护：mode_ai==0 丢失目标，保持AI模式但锁定当前姿态 */
   Gimbal_SetMode(g, g_cmd->mode);
 
   /* 处理yaw控制命令，软件限位 */
@@ -215,8 +216,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
   }
   CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
    
-  if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) {
+  if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL && g_cmd->mode_ai != 0) {
-    /* AI 模式：直接从指令获取角度设定值（每周期刷新） */
+    /* AI 模式且有目标(mode_ai==1/2)：跟踪，用AI下发的setpoint */
     g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
     g->setpoint.eulr.pit = g_cmd->setpoint_pit;
   } else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
@@ -252,25 +253,35 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
 
     break;
   case GIMBAL_MODE_AI_CONTROL:
-    /* --- YAW --- */
+    if (g_cmd->mode_ai == 0) {
-    // 位置环: Kp * (pos_ref - pos_fdb)
+      /* 丢失目标：setpoint保持不变，用普通PID锁住当前姿态，无前馈 */
-    yaw_omega_set_point = PID_Calc(&(g->pid.aimbot.yaw_angle),
+      yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
-                                   g->setpoint.eulr.yaw,
+                                     g->feedback.imu.eulr.yaw, 0.0f, g->dt);
-                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
+      g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
-    // 速度环: Kd * (vel_ref - vel_fdb)，用上位机下发的 yaw_vel 作为前馈参考
+                            g->feedback.imu.gyro.z, 0.f, g->dt);
-    g->out.yaw = PID_Calc(&(g->pid.aimbot.yaw_omega),
-                          yaw_omega_set_point + g_cmd->ff_vel_yaw,
-                          g->feedback.imu.gyro.z, 0.0f, g->dt)
-               + g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA;  // 加速度前馈: J * acc
 
-    /* --- PITCH --- (反馈轴与 ABSOLUTE 模式一致: eulr.rol / gyro.y) */
+      pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
-    pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle),
+                                      g->feedback.imu.eulr.rol, 0.0f, g->dt);
-                                   g->setpoint.eulr.pit,
+      g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
-                                   g->feedback.imu.eulr.rol, 0.0f, g->dt);
+                            g->feedback.imu.gyro.y, 0.f, g->dt);
-    g->out.pit = PID_Calc(&(g->pid.aimbot.pit_omega),
+    } else {
-                          pit_omega_set_point + g_cmd->ff_vel_pit,
+      /* mode_ai==1/2：跟踪目标，带前馈 */
-                          g->feedback.imu.gyro.y, 0.0f, g->dt)
+      /* --- YAW --- */
-               + g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA;  // 加速度前馈: J * acc
+      yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+                                     g->feedback.imu.eulr.yaw, 0.0f, g->dt);
+      yaw_omega_set_point += g_cmd->ff_vel_yaw;
+      g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
+                            g->feedback.imu.gyro.z, 0.f, g->dt);
+      g->out.yaw += g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA;
+
+      /* --- PIT --- */
+      pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), -g->setpoint.eulr.pit,
+                                     g->feedback.imu.eulr.rol, 0.0f, g->dt);
+      pit_omega_set_point -= g_cmd->ff_vel_pit;
+      g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+                            g->feedback.imu.gyro.y, 0.f, g->dt);
+      g->out.pit -= g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA;
+    }
     break;
   case GIMBAL_MODE_RECOVER:
     g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw,

User/module/gimbal.h

@@ -41,6 +41,7 @@ typedef struct {
 
 typedef struct {
   Gimbal_Mode_t mode;
+  uint8_t mode_ai; /* AI模式细分，0表示不控制，1表示控制云台但不开火，2表示控制云台且开火 */
   float delta_yaw;
   float delta_pit;
     /* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */

User/module/shoot.h

@@ -102,6 +102,7 @@ typedef struct {
 }Shoot_JamDetection_t;
 
 typedef struct {
+  bool ref_online; /* 裁判系统数据是否在线 */
   /* 从裁判系统读取的常量 */
   float Hmax;  // 热量上限
   float Hcd;   // 热量冷却速度

User/task/ctrl_chassis.c

@@ -31,6 +31,7 @@ Chassis_CMD_t chassis_cmd = {
 };
 Chassis_IMU_t chassis_imu;
 Referee_ForChassis_t chassis_ref; /* 裁判系统底盘数据 */
+Chassis_RefereeUI_t chassis_ui; /* 发送到裁判系统UI的数据结构 */
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -101,13 +102,15 @@ void Task_ctrl_chassis(void *argument) {
     Chassis_UpdateFeedback(&chassis);
 		osMessageQueueGet(task_runtime.msgq.cap.power_limit, &chassis.power_limit, NULL, 0);
     Chassis_Control(&chassis, &chassis_cmd);  
-
+    Chassis_DumpUI(&chassis, &chassis_ui);
     ///* 功率限制：裁判系统在线时使用下发上限，否则使用保守默认值 */
     // float power_limit = (chassis_ref.ref_status == REF_STATUS_RUNNING)
     //                     ? chassis_ref.chassis_power_limit
     //                     : 500.0f;
      //(&chassis, power_limit);
-
+osMessageQueueReset(task_runtime.msgq.referee.ui.tochassis); // 重置消息队列，防止阻塞
+    osMessageQueuePut(task_runtime.msgq.referee.ui.tochassis, &chassis_ui, 0, 0);
+    /* 将底盘状态UI数据发送到裁判系统进程，由裁判系统进程负责调用
 
     osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞
     osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0);

User/task/ctrl_shoot.c

@@ -4,6 +4,7 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
+#include "cmsis_os.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "device/mrobot.h"
@@ -20,6 +21,7 @@
 Shoot_t shoot;
 Shoot_CMD_t shoot_cmd;
 Referee_ForShoot_t shoot_ref;
+Shoot_AI_t shoot_forai;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -71,11 +73,17 @@ void Task_ctrl_shoot(void *argument) {
     osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
     osMessageQueueGet(task_runtime.msgq.shoot.ref, &shoot_ref, NULL, 0);
     
+
+    Shoot_DumpAI(&shoot, &shoot_forai);
+    osMessageQueuePut(task_runtime.msgq.ai.rawdata_FromShoot, &shoot_forai, 0, 0);
     if (shoot_ref.ref_status == REF_STATUS_RUNNING) {
+      shoot.heatcontrol.ref_online = true;
       shoot.heatcontrol.Hmax = (float)shoot_ref.robot_status.shooter_barrel_heat_limit;
       shoot.heatcontrol.Hcd  = (float)shoot_ref.robot_status.shooter_barrel_cooling_value;
       shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat;
-      shoot.heatcontrol.Hgen = 100.0f; /* 42mm弹丸每发产生热量 */
+      shoot.heatcontrol.Hgen = 10.0f; /* 42mm弹丸每发产生热量 */
+    } else {
+      shoot.heatcontrol.ref_online = false;
     }
 	  Shoot_UpdateFeedback(&shoot);
     Shoot_SetMode(&shoot,shoot_cmd.mode);

User/task/init.c

@@ -10,8 +10,10 @@
 #include "module/shoot.h"
 #include "module/balance_chassis.h"
 #include "module/gimbal.h"
+#include "module/cmd/cmd.h"
 #include "device/dr16.h"
 #include "device/referee.h"
+#include "module/aimbot.h"
 
 /* USER INCLUDE END */
 
@@ -62,7 +64,7 @@ void Task_Init(void *argument) {
   task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
   
   task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL);
-	
+  task_runtime.msgq.cmd.nuc= osMessageQueueNew(2u, sizeof(Aimbot_AIResult_t), NULL);
   /* AI */
   task_runtime.msgq.ai.rawdata_FromGimbal = osMessageQueueNew(2u, sizeof(Gimbal_Feedback_t), NULL);
   task_runtime.msgq.ai.rawdata_FromIMU = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);