From 523062d89d2fb2e94c7cc8967739aa18d04b078b Mon Sep 17 00:00:00 2001 From: asd Date: Wed, 22 Jul 2026 15:09:00 +0800 Subject: [PATCH] =?UTF-8?q?=E5=BA=95=E7=9B=98=E8=83=BD=E8=BF=9B=E8=A1=8C?= =?UTF-8?q?=E5=89=8D=E5=90=8E=E5=B7=A6=E5=8F=B3=E8=BF=90=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .settings/bundles-lock.store.json | 88 ++++++++++++++++++++ .settings/bundles.store.json | 8 ++ .vscode/launch.json | 23 ++++++ Core/Inc/chassis_control.h | 23 +++++- Core/Inc/remote_control.h | 1 + Core/Src/chassis_control.c | 133 +++++++++++++++++++++++------- Core/Src/freertos.c | 46 +++++++---- Core/Src/remote_control.c | 39 +++++++-- 8 files changed, 306 insertions(+), 55 deletions(-) create mode 100644 .vscode/launch.json diff --git a/.settings/bundles-lock.store.json b/.settings/bundles-lock.store.json index 394ecb1..996a296 100644 --- a/.settings/bundles-lock.store.json +++ b/.settings/bundles-lock.store.json @@ -154,6 +154,50 @@ } ] }, + { + "name": "programmer", + "version": "2.23.0", + "platform": "aarch64-darwin", + "selected_by": [ + { + "name": "programmer", + "version": "2.23.0" + } + ] + }, + { + "name": "programmer", + "version": "2.23.0", + "platform": "x86_64-darwin", + "selected_by": [ + { + "name": "programmer", + "version": "2.23.0" + } + ] + }, + { + "name": "programmer", + "version": "2.23.0", + "platform": "x86_64-linux", + "selected_by": [ + { + "name": "programmer", + "version": "2.23.0" + } + ] + }, + { + "name": "programmer", + "version": "2.23.0", + "platform": "x86_64-windows", + "selected_by": [ + { + "name": "programmer", + "version": "2.23.0" + } + ] + }, { "name": "st-arm-clangd", "version": "19.1.2+st.3", @@ -186,6 +230,50 @@ "version": "19.1.2+st.3" } ] + }, + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2", + "platform": "aarch64-darwin", + "selected_by": [ + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2" + } + ] + }, + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2", + "platform": "x86_64-darwin", + "selected_by": [ + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2" + } + ] + }, + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2", + "platform": "x86_64-linux", + "selected_by": [ + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2" + } + ] + }, + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2", + "platform": "x86_64-windows", + "selected_by": [ + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2" + } + ] } ] } diff --git a/.settings/bundles.store.json b/.settings/bundles.store.json index 19c686e..4d000d2 100644 --- a/.settings/bundles.store.json +++ b/.settings/bundles.store.json @@ -19,6 +19,14 @@ { "name": "jlink-gdbserver", "version": "9.24.0+st.1" + }, + { + "name": "programmer", + "version": "2.23.0" + }, + { + "name": "stlink-gdbserver", + "version": "7.14.0+st.2" } ] } diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..8d5d847 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,23 @@ +{ + // 使用 IntelliSense 了解相关属性。 + // 悬停以查看现有属性的描述。 + // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "stlinkgdbtarget", + "request": "launch", + "name": "STM32Cube: Launch ST-Link GDB Server", + "origin": "snippet", + "cwd": "${workspaceFolder}", + "preBuild": "${command:st-stm32-ide-debug-launch.build}", + "runEntry": "main", + "imagesAndSymbols": [ + { + "imageFileName": "${command:st-stm32-ide-debug-launch.get-projects-binary-from-context1}" + } + ] + } + ] +} \ No newline at end of file diff --git a/Core/Inc/chassis_control.h b/Core/Inc/chassis_control.h index 9d196c2..c89e263 100644 --- a/Core/Inc/chassis_control.h +++ b/Core/Inc/chassis_control.h @@ -4,9 +4,28 @@ #include "pid.h" #include "main.h" +#define LENGTH_A 200.0f // 底盘长度一半 (mm) +#define LENGTH_B 166.0f // 底盘宽度一半 (mm) +#define WHEEL_PERIMETER 0.152f // 麦轮周长 (m) +#define CHASSIS_DECELE_RATIO 19.0f // 底盘减速比 +#define MAX_CHASSIS_SPEED 2.0f // 底盘最大线速度(m/s)，摇杆推满对应此值 +#define RC_DEADZONE 10 // 摇杆死区，绝对值小于此值视为0 + +typedef enum { + CHASSIS_NORMAL = 0, // 正常模式：遥控器控制底盘不跟随云台 + CHASSIS_GYROSCOPE = 1, // 小陀螺模式：底盘原地旋转 + CHASSIS_SLOW = 2, // 停止模式：底盘不动 +} ChassisMode; + +typedef struct { + float vx; // 前后方向速度 (m/s) + float vy; // 左右方向速度 (m/s) + float vw; // 旋转角速度 (rad/s) +} Chassis_Speed; + void chassis_control_init(void); void chassis_control_task(void); - -void chassis_set_speed(int16_t m1, int16_t m2, int16_t m3, int16_t m4); +void chassis_set_target_speed(const Chassis_Speed *speed); +void chassis_set_mode(ChassisMode mode); #endif \ No newline at end of file diff --git a/Core/Inc/remote_control.h b/Core/Inc/remote_control.h index 91cfc60..2e1b155 100644 --- a/Core/Inc/remote_control.h +++ b/Core/Inc/remote_control.h @@ -101,5 +101,6 @@ extern void remote_control_init(void); */ extern const RC_ctrl_t *get_remote_control_point(void); extern void RC_USART3_IRQHandler(void); +extern uint8_t is_rc_online(void); // 检测遥控器是否在线 #endif \ No newline at end of file diff --git a/Core/Src/chassis_control.c b/Core/Src/chassis_control.c index 0de8364..6b0bf0e 100644 --- a/Core/Src/chassis_control.c +++ b/Core/Src/chassis_control.c @@ -1,54 +1,127 @@ #include "chassis_control.h" #include "CAN_receive.h" +#include "remote_control.h" +#include +#include -static pid_t pid_motor1; -static pid_t pid_motor2; -static pid_t pid_motor3; -static pid_t pid_motor4; +#define PI 3.1415926535f -static int16_t target_speed_m1; -static int16_t target_speed_m2; -static int16_t target_speed_m3; -static int16_t target_speed_m4; +static pid_t pid_motor[4]; // 四个底盘电机的PID控制器 +static int16_t target_speed[4]; // 四个轮子的目标转速(rpm) +static Chassis_Speed chassis_speed; // 底盘目标速度(vx,vy,vw) +static ChassisMode chassis_mode = CHASSIS_NORMAL; // 底盘当前模式 + +static void omni_calc(Chassis_Speed *speed, int16_t *out_speed); +static void absolute_cal(Chassis_Speed *absolute_speed, float angle); /** * @brief 初始化底盘电机的PID - * 从左到右依次是：PID作用模块，KP,KI,KD,电机最大输出，电机最大限制输出 */ void chassis_control_init(void) { - pid_init(&pid_motor1, 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);//底盘电机1 - pid_init(&pid_motor2, 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);//底盘电机2 - pid_init(&pid_motor3, 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);//底盘电机3 - pid_init(&pid_motor4, 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);//底盘电机4 + for (int i = 0; i < 4; i++) + { + pid_init(&pid_motor[i], 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f); + } + memset(target_speed, 0, sizeof(target_speed)); + memset(&chassis_speed, 0, sizeof(chassis_speed)); } /** - * @brief 控制底盘电机的转速 + * @brief 设置底盘目标速度（vx, vy, vw） + */ +void chassis_set_target_speed(const Chassis_Speed *speed) +{ + if (speed == NULL) return; + chassis_speed.vx = speed->vx; + chassis_speed.vy = speed->vy; + chassis_speed.vw = speed->vw; +} + +/** + * @brief 设置底盘模式 + */ +void chassis_set_mode(ChassisMode mode) +{ + chassis_mode = mode; +} + +/** + * @brief 底盘控制主循环：全向轮运动学解算 + PID + CAN发送 */ void chassis_control_task(void) { - const motor_measure_t *m1 = get_chassis_motor_measure_point(0); - const motor_measure_t *m2 = get_chassis_motor_measure_point(1); - const motor_measure_t *m3 = get_chassis_motor_measure_point(2); - const motor_measure_t *m4 = get_chassis_motor_measure_point(3); + // 遥控器掉线检测：超过100ms无信号，停止CAN发送，电机随惯性停下 + if (!is_rc_online()) + { + for (int i = 0; i < 4; i++) + { + pid_motor[i].integral = 0; // 清空PID积分，防止重新上线时积分饱和 + pid_motor[i].last_error = 0; // 清空上次误差 + } + memset(target_speed, 0, sizeof(target_speed)); // 目标转速清零 + return; // 不发送CAN信号，电机随惯性停下 + } + const motor_measure_t *m[4]; + m[0] = get_chassis_motor_measure_point(0); + m[1] = get_chassis_motor_measure_point(1); + m[2] = get_chassis_motor_measure_point(2); + m[3] = get_chassis_motor_measure_point(3); - //pid运算 - int16_t cur1 = (int16_t)pid_calc(&pid_motor1, target_speed_m1, m1->speed_rpm); - int16_t cur2 = (int16_t)pid_calc(&pid_motor2, target_speed_m2, m2->speed_rpm); - int16_t cur3 = (int16_t)pid_calc(&pid_motor3, target_speed_m3, m3->speed_rpm); - int16_t cur4 = (int16_t)pid_calc(&pid_motor4, target_speed_m4, m4->speed_rpm); + absolute_cal(&chassis_speed, 0); - CAN_cmd_chassis(cur1, cur2, cur3, cur4); + int16_t cur[4]; + for (int i = 0; i < 4; i++) + { + cur[i] = (int16_t)pid_calc(&pid_motor[i], (float)target_speed[i], (float)m[i]->speed_rpm); + } + + CAN_cmd_chassis(cur[0], cur[1], cur[2], cur[3]); } /** - * @brief 设置电机转速的电流值，范围（0~16384） + * @brief 全向轮运动学解算：将 vx,vy,vw 转换为四个轮子的转速 + * 轮子编号：0=左, 1=前, 2=右, 3=后 */ -void chassis_set_speed(int16_t m1, int16_t m2, int16_t m3, int16_t m4) +static void omni_calc(Chassis_Speed *speed, int16_t *out_speed) { - target_speed_m1 = m1; - target_speed_m2 = m2; - target_speed_m3 = m3; - target_speed_m4 = m4; + float wheel_rpm_ratio; // 速度(m/s)到转速(rpm)的转换系数 + float L; // 底盘中心到轮子的距离 = LENGTH_A + LENGTH_B + int16_t wheel_rpm[4]; + + // 60 / 轮周长 * 减速比 = 转速转换系数 (m/s → rpm) + // 例如：vx=1m/s → 轮子60/0.152/19=7500rpm(电机端) + wheel_rpm_ratio = 60.0f / WHEEL_PERIMETER * CHASSIS_DECELE_RATIO; + L = LENGTH_A + LENGTH_B; + + // 轮子编号：基于用户观察修正 + // M1(0x201)=左前, M2(0x202)=左后, M3(0x203)=右前, M4(0x204)=右后 + // 物理层vy符号：+, -, -, + 是X型麦轮的标准平移公式（右侧电机反向安装，代码层vy: +, -, +, -） + // 四个轮子对角配对：左前+右后同向，右前+左后同向，两组反向 + wheel_rpm[0] = (int16_t)(( speed->vx + speed->vy + speed->vw * L) * wheel_rpm_ratio); // 左前轮 + wheel_rpm[1] = (int16_t)(( speed->vx - speed->vy + speed->vw * L) * wheel_rpm_ratio); // 左后轮 + wheel_rpm[2] = (int16_t)((-speed->vx + speed->vy + speed->vw * L) * wheel_rpm_ratio); // 右前轮 + wheel_rpm[3] = (int16_t)((-speed->vx - speed->vy + speed->vw * L) * wheel_rpm_ratio); // 右后轮 + + memcpy(out_speed, wheel_rpm, 4 * sizeof(int16_t)); +} + +/** + * @brief 坐标系变换：将遥控器坐标系的速度转换到底盘坐标系 + * @param absolute_speed 遥控器输入的目标速度 + * @param angle 云台相对于底盘的角度（度） + */ +static void absolute_cal(Chassis_Speed *absolute_speed, float angle) +{ + float angle_hd; // 角度转弧度 + Chassis_Speed temp_speed; // 旋转后的临时速度 + + angle_hd = angle * PI / 180.0f; // 角度转弧度 + + // 坐标系旋转矩阵：将遥控器坐标系速度旋转到当前底盘坐标系 + temp_speed.vw = absolute_speed->vw; + temp_speed.vx = absolute_speed->vx * cosf(angle_hd) - absolute_speed->vy * sinf(angle_hd); + temp_speed.vy = absolute_speed->vx * sinf(angle_hd) + absolute_speed->vy * cosf(angle_hd); + + omni_calc(&temp_speed, target_speed); // 全向轮解算，结果存入target_speed } \ No newline at end of file diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index f150519..2388826 100644 --- a/Core/Src/freertos.c +++ b/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 */ diff --git a/Core/Src/remote_control.c b/Core/Src/remote_control.c index 4e0ea28..22d14f3 100644 --- a/Core/Src/remote_control.c +++ b/Core/Src/remote_control.c @@ -1,10 +1,10 @@ /** ****************************(C) COPYRIGHT 2019 DJI**************************** * @file remote_control.c/h - * @brief ?????????????????????????SBUS?????????DMA????????CPU - * ????????????????????????????????????????????DMA?????? - * ????????????????? - * @note ???????????????????????????freeRTOS???? + * @brief ?????????????????????????SBUS??��?��??????DMA????????CPU + * ????????????????��????????????????????��????????DMA?????? + * ??????????��??????? + * @note ????????????????��???????????freeRTOS???? * @history * Version Date Author Modification * V1.0.0 Dec-01-2019 RM 1. ??? @@ -32,7 +32,7 @@ extern DMA_HandleTypeDef hdma_usart3_rx; * @retval none */ /** - * @brief ?????????? + * @brief ?????��????? * @param[in] sbus_buf: ?????????? * @param[out] rc_ctrl: ?????????? * @retval none @@ -47,6 +47,9 @@ RC_ctrl_t rc_ctrl; //????????????18??????????36????????????DMA??????? static uint8_t sbus_rx_buf[2][SBUS_RX_BUF_NUM]; +//遥控器掉线检测：记录最后一次成功解析的时间戳(ms) +static volatile uint32_t rc_last_update_tick = 0; + /** * @brief remote control init * @param[in] none @@ -77,7 +80,7 @@ const RC_ctrl_t *get_remote_control_point(void) } -//?????? +//?????��? void RC_USART3_IRQHandler(void) { if(huart3.Instance->SR & UART_FLAG_RXNE)//????????? @@ -95,7 +98,7 @@ void RC_USART3_IRQHandler(void) /* Current memory buffer used is Memory 0 */ //disable DMA - //?DMA + //?��DMA __HAL_DMA_DISABLE(&hdma_usart3_rx); //get receive data length, length = set_data_length - remain_length @@ -123,7 +126,7 @@ void RC_USART3_IRQHandler(void) { /* Current memory buffer used is Memory 1 */ //disable DMA - //?DMA + //?��DMA __HAL_DMA_DISABLE(&hdma_usart3_rx); //get receive data length, length = set_data_length - remain_length @@ -159,7 +162,7 @@ void RC_USART3_IRQHandler(void) * @retval none */ /** - * @brief ?????????? + * @brief ?????��????? * @param[in] sbus_buf: ?????????? * @param[out] rc_ctrl: ?????????? * @retval none @@ -191,4 +194,22 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl) rc_ctrl->rc.ch[2] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[3] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET; + + //更新遥控器最后一次在线时间戳，用于掉线检测 + rc_last_update_tick = HAL_GetTick(); +} + +/** + * @brief check if remote control is online + * @param[in] none + * @retval 1: online, 0: offline (no data for > 100ms) + */ +/** + * @brief 检测遥控器是否在线 + * @param[in] none + * @retval 1: 在线, 0: 掉线(超过100ms无数据) + */ +uint8_t is_rc_online(void) +{ + return (HAL_GetTick() - rc_last_update_tick) < 100; } \ No newline at end of file