底盘能进行前后左右运动

Core/Src/freertos.c

@@ -75,7 +75,7 @@ const osThreadAttr_t Command_Task_attributes = {
 };
 /* Definitions for chassis_speed_queue */
 osMessageQueueId_t chassis_speed_queueHandle;
-uint8_t chassis_speed_queueBuffer[ 5 * sizeof( uint16_t ) ];
+uint8_t chassis_speed_queueBuffer[ 5 * sizeof( Chassis_Speed ) ];  // 队列缓冲区：5个Chassis_Speed(vx,vy,vw)
 osStaticMessageQDef_t chassis_speed_queueControlBlock;
 const osMessageQueueAttr_t chassis_speed_queue_attributes = {
   .name = "chassis_speed_queue",
@@ -120,7 +120,7 @@ void MX_FREERTOS_Init(void) {
 
   /* Create the queue(s) */
   /* creation of chassis_speed_queue */
-  chassis_speed_queueHandle = osMessageQueueNew (5, sizeof(uint16_t), &chassis_speed_queue_attributes);
+  chassis_speed_queueHandle = osMessageQueueNew (5, sizeof(Chassis_Speed), &chassis_speed_queue_attributes);
 
   /* USER CODE BEGIN RTOS_QUEUES */
   /* add queues, ... */
@@ -175,17 +175,20 @@ void StartCAN_Receive_Task(void *argument)
 void StartChassis_Control_Task(void *argument)
 {
   /* USER CODE BEGIN StartChassis_Control_Task */
-  chassis_control_init();
-  int16_t speed = 0;
+  chassis_control_init();          // 初始化底盘电机PID
+  Chassis_Speed speed;             // 接收遥控器下发的目标速度
+
   /* Infinite loop */
   for(;;)
   {
+    // 非阻塞接收遥控器速度指令，有新的就更新目标速度
     if (osMessageQueueGet(chassis_speed_queueHandle, &speed, NULL, 0) == osOK)
     {
-      chassis_set_speed(speed, 0, 0, 0);
+      chassis_set_target_speed(&speed);
     }
+    // 执行底盘控制：运动学解算 + PID计算 + CAN发送
     chassis_control_task();
-    osDelay(1);
+    osDelay(1);                    // 1ms 控制周期
   }
   /* USER CODE END StartChassis_Control_Task */
 }
@@ -202,17 +205,32 @@ void StartCommand_Task(void *argument)
   /* USER CODE BEGIN StartCommand_Task */
   remote_control_init();
   const RC_ctrl_t *rc;
-  int16_t speed;
+  Chassis_Speed speed;
   /* Infinite loop */
   for(;;)
   {
-    rc = get_remote_control_point();//rc=获取遥控器数据
-    speed = (int16_t)((float)rc->rc.ch[3] * 5000.0f / 660.0f);//根据通道三的值，映射到-5000到5000之间
-    if (speed > 5000) 
-      speed = 5000;
-    if (speed < -5000) 
-      speed = -5000;
-    osMessageQueuePut(chassis_speed_queueHandle, &speed, 0, 0);
+    rc = get_remote_control_point();  // 获取遥控器数据
+
+    // 遥控器通道映射：ch[3]=左摇杆上下(前后), ch[2]=左摇杆左右(左右)
+    // 范围 [-660, 660] 映射到 [-MAX_CHASSIS_SPEED, MAX_CHASSIS_SPEED] m/s
+    // 摇杆死区：RC_DEADZONE 以内视为0，消除摇杆中位抖动
+    speed.vx =  (abs(rc->rc.ch[2]) > RC_DEADZONE) ? (float)rc->rc.ch[3] * MAX_CHASSIS_SPEED / 660.0f : 0.0f;  // 前后速度
+    speed.vy =  (abs(rc->rc.ch[3]) > RC_DEADZONE) ? (float)rc->rc.ch[2] * MAX_CHASSIS_SPEED / 660.0f : 0.0f;  // 左右速度
+    speed.vw =  0;  // 旋转速度默认为0
+
+    // 右侧开关切换模式
+    if (rc->rc.s[1] == RC_SW_UP)
+    {
+      speed.vw = -2.0f;  // 上拨：小陀螺模式，原地旋转
+    }
+    else if (rc->rc.s[1] == RC_SW_DOWN)
+    {
+      speed.vx = 0;  // 下拨：停止模式，所有速度置零
+      speed.vy = 0;
+      speed.vw = 0;
+    }
+
+    osMessageQueuePut(chassis_speed_queueHandle, &speed, 0, 0);  // 发送速度到底盘控制任务
     osDelay(10);
   }
   /* USER CODE END StartCommand_Task */