辅助行走机器人开发 —— 下位机控制

---

辅助行走机器人开发项目总结

总体设计

项目分为语音模式、导航模式和手动模式。

• 语音模式、导航模式为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();

中断初始化

1. 串口2：APP（蓝牙）
2. 串口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、三轴速度2*3byte、三轴加速度2*3byte、三轴角速度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;   // 复位
	
	// 验证帧尾
	if(Receive_Data.buffer[10] == FRAME_TAIL) {
		// 将所有的模式的标志位设0，此时为usart3控制模式		
		PS2_ON_Flag=0;
		APP_ON_Flag=0;
		Usart_ON_Flag=0;
		Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
		Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
}

之后Move_X和Move_Y被输入Drive_Motor()运动学分析函数，从而输出pwm波控制车辆。

在 7 种控制机器人方式中，ROS 的控制优先级是最高的，在任意时候 串口 3 接收到数据，则强制进入 ROS 控制模式。