文章目录
- 总体设计
- 硬件选型
- 控制模式
- 程序流程
- 1 初始化
- 2 FreeRTOS
- Balance_task
- MPU6050_task
- Show_task
- Led_task
- Ps2_task
- Data_task
- 3 中断
- 串口中断
辅助行走机器人开发项目总结
总体设计
项目分为语音模式、导航模式和手动模式。
- 语音模式、导航模式为ROS下发控制指令；
- 手动模式由手柄下发运动指令。
硬件选型
工控机：NUC11
传感器：单线雷达rplidar a1、Kinect
MCU: STM32F407
IMU: MPU6050
电机驱动：D50A（单片机通过电机驱动电路输出PWM波对电机速度进行控制）
编码器
控制模式
- 手柄控制
- APP控制
- ROS控制（串口通信）
关闭JTAG接口 提高鲁棒性
打开SWD接口调试
中断优先级分组设置
初始化LED、蜂鸣器、OLED
使能开关初始化（车辆是否接收控制开关）
复位按键和用户按键初始化（更新陀螺仪零点）
串口2、3（ROS使用串口3）初始化
uart3_init(115200);
程序流程
1 初始化
硬件初始化：
systemInit();
中断初始化
- 串口2：APP（蓝牙）
- 串口3：ROS控制命令获取
2 FreeRTOS
目的：
创建多个任务并行运行，并分配频率和优先级（单片机资源有限，使用RTOS框架）单片机本身中断的优先级最高。
FreeRTOS初始化task
// 创建任务
xTaskCreate((TaskFunction_t )start_task,
(const char* )"start_task",
(uint16_t )START_STK_SIZE, // 任务堆栈大小
(void* )NULL, // 传递参数
(UBaseType_t )START_TASK_PRIO, // 优先级
(TaskHandle_t* )&StartTask_Handler); // 任务句柄
vTaskStartScheduler(); // 开启任务调度
void start_task(void *pvParameters)
{
taskENTER_CRITICAL(); //Enter the critical area
//Create the task
xTaskCreate(Balance_task, "Balance_task", BALANCE_STK_SIZE, NULL, BALANCE_TASK_PRIO, NULL);
xTaskCreate(MPU6050_task, "MPU6050_task", MPU6050_STK_SIZE, NULL, MPU6050_TASK_PRIO, NULL);
xTaskCreate(show_task, "show_task", SHOW_STK_SIZE, NULL, SHOW_TASK_PRIO, NULL);
xTaskCreate(led_task, "led_task", LED_STK_SIZE, NULL, LED_TASK_PRIO, NULL);
xTaskCreate(pstwo_task, "PSTWO_task", PS2_STK_SIZE, NULL, PS2_TASK_PRIO, NULL);
xTaskCreate(data_task, "DATA_task", DATA_STK_SIZE, NULL, DATA_TASK_PRIO, NULL);
vTaskDelete(StartTask_Handler);
taskEXIT_CRITICAL();
}
子任务（如Balance_task、MPU6050_task等）不会被直接删除。它们会在任务执行完毕后自动退出，或者由其他任务或中断删除。
Balance_task
运动控制任务
10ms 4级优先
获取编码器的数据Get_Velocity_Form_Encoder()，通过运动学正解算转换为速度，之后发送给上位机进行里程计解算。
以上任何一个地方获取控制命令后，都会进入Balance_task()中进行控制
将控制命令转换为三轴的目标速度，输入Drive_Motor()运动学分析函数，之后运动学逆解算计算出车轮所需速度(pwm)。
// 解算计算得出的pwm
Set_Pwm(-MOTOR_A.Motor_Pwm,-MOTOR_B.Motor_Pwm, 0);
// 设置pwm
void Set_Pwm(int motor_a,int motor_b,int servo)
{
if(motor_a<0) AIN2=0, AIN1=1;
else AIN2=1, AIN1=0;
TIM_SetCompare4(TIM8,myabs(motor_a));
if(motor_b>0) BIN2=0, BIN1=1;
else BIN2=1, BIN1=0;
TIM_SetCompare3(TIM8,myabs(motor_b));
}
TIM_SetCompare3()用来设置定时器的比较值，从而控制对应通道的PWM信号的占空比。
MPU6050_task
加速度角速度获取任务
100Hz 3级优先
使用I2C通信读取MPU6050的X、Y、Z轴加表和陀螺仪的原始数据，分别保存在acc、gyro数组中。读取过程中，高位和低位数据通过位移操作进行合并。
获取陀螺仪数据
gyro[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L);
gyro[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);
gyro[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);
之后将加表和陀螺仪数据发送给上位机进行里程计解算。
Show_task
OLED显示和串口3（默认开启，与ROS通信）发送数据任务、蜂鸣器叫任务
50Hz 3级优先
显示当前控制模式，默认ROS
Led_task
LED闪烁
3级优先
Ps2_task
获取手柄控制任务
100Hz 4级优先
读取按键值，读取遥感的模拟量
Data_task
串口3（ROS）发送给上位机
20Hz 4级优先
发送数据：
帧头1byte、底盘使能标识1byte、三轴速度23byte、三轴加速度23byte、三轴角速度2*3byte、数据校验位1byte、帧尾1byte。
串口一次只能发送一个 8 位（1 个字节）的数据，2 个字节（short）的数据会拆分成高 8 位和低 8 位发送。
Send_Data.buffer[n]=Send_Data.Sensor_Str.Gyroscope.X_data>>8;
Send_Data.buffer[n+1]=Send_Data.Sensor_Str.Gyroscope.X_data;
之后发送22byte的数据：
void USART3_SEND(void)
{
unsigned char i = 0;
for(i=0; i<22; i++)
{
usart3_send(Send_Data.buffer[i]);
}
}
// 子函数
void usart3_send(u8 data)
{
USART3->DR = data;
while((USART3->SR&0x40)==0);
}
USART3->DR = data;: 这行代码将传递给函数的 data 值写入 USART3 外设的数据寄存器 DR（Data Register）。这会将数据加载到 USART3 的发送缓冲区中，准备发送。
while((USART3->SR&0x40)==0);: 这是一个循环，用于等待 USART3 外设发送完成。在 STM32 的串口通信中，发送完成时，SR（Status Register）寄存器的第6位（0x40）会被置为1。这个循环不断检查 SR 寄存器的状态，直到发送完成。
它将数据存储到 USART3 的数据寄存器，然后等待发送完成，以确保数据成功发送出去。
浮点数成1000，并强制转换位short（保留3位小数）。之后上位机端在接收到数据后，将接收到两个 8 位数据合并后转换为 short 型，在缩小一千倍来进行单位的转换。
3 中断
串口中断
usartx.c
串口2中断
APP控制
判断是否接收到数据
之后判断帧头来选择电机操作
串口3中断
生成x，z轴的目标速度
之后提供给balace_task()中的Drive_Motor()
int USART3_IRQHandler():
判断是否接收到数据
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
读取数据
Usart_Receive = USART_ReceiveData(USART3);
检查帧头
if(Usart_Receive == FRAME_HEADER||Count>0)
Count++;
else
Count=0;
数据包的长度为11
if (Count == 11){
Count = 0;
帧尾验证
}
在 7 种控制机器人方式中，ROS 的控制优先级是最高的，在任意时候 串口 3 接收到数据，则强制进入 ROS 控制模式。
