asd/Infantry_new
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

底盘能进行前后左右运动

Browse Source
This commit is contained in:
Wei Zitong
2026-07-22 15:09:00 +08:00
parent 15c5074a0a
commit 523062d89d
8 changed files with 306 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
.settings/bundles-lock.store.json
View File

@@ -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"
}
]
}
]
}

8
.settings/bundles.store.json
View File

@@ -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"
}
]
}

23
.vscode/launch.json vendored Normal file
View File

@@ -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}"
}
]
}
]
}

23
Core/Inc/chassis_control.h
View File

@@ -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

1
Core/Inc/remote_control.h
View File

@@ -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

133
Core/Src/chassis_control.c
View File

@@ -1,54 +1,127 @@
#include "chassis_control.h"
#include "CAN_receive.h"
#include "remote_control.h"
#include <math.h>
#include <string.h>
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
}

46
Core/Src/freertos.c
View File

@@ -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 */

39
Core/Src/remote_control.c
View File

@@ -1,10 +1,10 @@
/**
****************************(C) COPYRIGHT 2019 DJI****************************
* @file remote_control.c/h
* @brief ?????????????????????????SBUS??<3F><>?<3F><>??????DMA????????CPU
* ????????????????<3F><>????????????????????<3F><>????????DMA??????
* ??????????<3F><>???????
* @note ????????????????<3F><>???????????freeRTOS????
* @brief ?????????????????????????SBUS??<3F><>?<3F><>??????DMA????????CPU
* ????????????????<3F><>????????????????????<3F><>????????DMA??????
* ??????????<3F><>???????
* @note ????????????????<3F><>???????????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 ?????<3F><>?????
* @brief ?????<3F><>?????
* @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)
}
//?????<3F><>?
//?????<3F><>?
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
//?<3F><>DMA
//?<3F><>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
//?<3F><>DMA
//?<3F><>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 ?????<3F><>?????
* @brief ?????<3F><>?????
* @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;
}