上ai

User/module/balance_chassis.c

@@ -222,7 +222,7 @@ static float Chassis_CalcSpringForce(float leg_length)
     const float L_AC = 0.042f;  // 弹簧上端高度
     const float L_BD = 0.050f;  // 杆BD长度
     const float L_BE = 0.254f;  // 杆BE长度
-    const float Fs = 360.0f *(360.0f/220.0f);    // 弹簧恒力 (N)
+    const float Fs = 360.0f;    // 弹簧恒力 (N)
     
     // 通过余弦定理计算∠ABE的余弦值
     float cos_theta = (L_AB*L_AB + L_BE*L_BE - leg_length*leg_length) / (2.0f * L_AB * L_BE);

User/module/config.c

@@ -24,9 +24,9 @@ Config_RobotParam_t robot_config = {
     .gimbal_param = {
         .pid = {
             .yaw_omega = {
-                .k = 2.0f,
+                .k = 0.25f,
                 .p = 1.0f,
-                .i = 0.3f,
+                .i = 0.0f,
                 .d = 0.0f,
                 .i_limit = 1.0f,
                 .out_limit = 1.0f,

User/module/gimbal.c

@@ -223,43 +223,55 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
     g->out.pit = 0.0f;
     break;
   case GIMBAL_MODE_AI_CONTROL:
-      if (g_ai != NULL && g_ai->ctrl) {
+    if (g_ai != NULL && g_ai->ctrl) {
       g->setpoint.eulr.yaw = g_ai->yaw;
-      g->setpoint.eulr.pit = g_ai->pit;
+      g->setpoint.eulr.pit = -g_ai->pit;
       
-      /* 限位处理 - 使用圆周误差保持一致性 */
+      /* 限位处理 - 需要考虑电机和IMU之间的偏差 */
       if (g->param->travel.yaw > 0) {
-        if (CircleError(g->setpoint.eulr.yaw, g->limit.yaw.max, M_2PI) > 0) {
+        float yaw_motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
-          g->setpoint.eulr.yaw = g->limit.yaw.max;
+        if (yaw_motor_imu_offset > M_PI) yaw_motor_imu_offset -= M_2PI;
+        if (yaw_motor_imu_offset < -M_PI) yaw_motor_imu_offset += M_2PI;
+        
+        /* 将IMU setpoint转换为电机角度后进行限位检查 */
+        float yaw_motor_target = g->setpoint.eulr.yaw + yaw_motor_imu_offset;
+        if (CircleError(yaw_motor_target, g->limit.yaw.max, M_2PI) > 0) {
+          g->setpoint.eulr.yaw = g->limit.yaw.max - yaw_motor_imu_offset;
         }
-        if (CircleError(g->setpoint.eulr.yaw, g->limit.yaw.min, M_2PI) < 0) {
+        if (CircleError(yaw_motor_target, g->limit.yaw.min, M_2PI) < 0) {
-          g->setpoint.eulr.yaw = g->limit.yaw.min;
+          g->setpoint.eulr.yaw = g->limit.yaw.min - yaw_motor_imu_offset;
         }
       }
+      
       if (g->param->travel.pit > 0) {
-        if (CircleError(g->setpoint.eulr.pit, g->limit.pit.max, M_2PI) > 0) {
+        float pit_motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
-          g->setpoint.eulr.pit = g->limit.pit.max;
+        if (pit_motor_imu_offset > M_PI) pit_motor_imu_offset -= M_2PI;
+        if (pit_motor_imu_offset < -M_PI) pit_motor_imu_offset += M_2PI;
+        
+        /* 将IMU setpoint转换为电机角度后进行限位检查 */
+        float pit_motor_target = g->setpoint.eulr.pit + pit_motor_imu_offset;
+        if (CircleError(pit_motor_target, g->limit.pit.max, M_2PI) > 0) {
+          g->setpoint.eulr.pit = g->limit.pit.max - pit_motor_imu_offset;
         }
-        if (CircleError(g->setpoint.eulr.pit, g->limit.pit.min, M_2PI) < 0) {
+        if (CircleError(pit_motor_target, g->limit.pit.min, M_2PI) < 0) {
-          g->setpoint.eulr.pit = g->limit.pit.min;
+          g->setpoint.eulr.pit = g->limit.pit.min - pit_motor_imu_offset;
         }
       }
     }
+    /* fallthrough - AI控制模式也需要执行PID计算 */
     
   case GIMBAL_MODE_ABSOLUTE:
   case GIMBAL_MODE_RELATIVE:
-    /* AI 控制模式：直接使用 AI 下发的目标角度 */
-
-    /* PID 控制计算 - AI 和手动控制都需要执行 */
     yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
                                    g->feedback.imu.eulr.yaw, 0.0f, g->dt);
     g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
-                          g->feedback.imu.gyro.z, 0.0f, g->dt);
+                          g->feedback.imu.gyro.z, 0.f, g->dt);
 
     pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
                                    g->feedback.imu.eulr.rol, 0.0f, g->dt);
     g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
-                          g->feedback.imu.gyro.y, 0.0f, g->dt);
+                          g->feedback.imu.gyro.y, 0.f, g->dt);
+
     break;
   }
   

User/task/ai.c

@@ -4,10 +4,12 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
+#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "bsp/fdcan.h"
 #include "module/config.h"
+#include "module/gimbal.h"
 #include "device/vision_bridge.h"
 /* USER INCLUDE END */
 
@@ -18,6 +20,7 @@
 /* USER STRUCT BEGIN */
 AI_cmd_t cmd_ai;
 AI_t ai;
+Gimbal_AI_t ai_for_gimbal;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -41,6 +44,15 @@ void Task_ai(void *argument) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
     AI_ParseCmdFromCan( &ai,&cmd_ai);
+    if (cmd_ai.mode != 0){
+      ai_for_gimbal.ctrl = true;
+    } else {
+      ai_for_gimbal.ctrl = false;
+    }
+    ai_for_gimbal.yaw = cmd_ai.gimbal.setpoint.yaw;
+    ai_for_gimbal.pit = cmd_ai.gimbal.setpoint.pit;
+    osMessageQueueReset(task_runtime.msgq.gimbal.ai_cmd);
+    osMessageQueuePut(task_runtime.msgq.gimbal.ai_cmd, &ai_for_gimbal, 0, 0); 
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/ctrl_gimbal.c

@@ -49,13 +49,13 @@ void Task_ctrl_gimbal(void *argument) {
     
     Gimbal_UpdateIMU(&gimbal);
     osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
-
+    osMessageQueueGet(task_runtime.msgq.gimbal.ai_cmd, &gimbal_ai, NULL, 0);
 
     Gimbal_UpdateFeedback(&gimbal);
 
     osMessageQueueReset(task_runtime.msgq.chassis.yaw); // 重置消息队列，防止阻塞
     osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
-    osMessageQueueGet(task_runtime.msgq.gimbal.ai_cmd, &gimbal_ai, NULL, 0);
+    
     Gimbal_Control(&gimbal, &gimbal_cmd, &gimbal_ai);
 
     Gimbal_Output(&gimbal);

User/task/init.c

@@ -49,6 +49,7 @@ void Task_Init(void *argument) {
   task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
   task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
   task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
+  task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核