此平衡小车基于STM32F401CCU6制作,在FreeRTOS下运行,能够完成直立,在手机蓝牙调试APP控制下能够前后运动以及转动。
基于STM32手把手教你做FreeRTOS平衡小车
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE
{
HAL_UART_Transmit(&huart1 , (uint8_t *)&ch, 1, 0xFFFF);
return ch;
}
struct HC05_PID_DATA//定义结构体
{
uint8_t LedStatus;
int8_t BalanceKP;
int8_t BalanceKD;
int8_t SpeedKP;
int8_t SpeedKI;
int8_t TurnKP;
int8_t TurnKD;
int8_t Speed_Set;
int8_t Turn_Set;
};
char VerifyData(uint8_t *data, uint8_t dataBIT);//数据校验
void GetPIDFromHC05(uint8_t *data);//逐字节读取数据
void Car_Init(void);//小车初始化,开启时序尽量不要变
/*开启串口接收
* 开启Encode
* OLED初始化
* MPU6050初始化
* 开启MPU6050中断
* 开启PWM
* 使能TB6612FNG*/
void Car_Task_PID_Control_20ms(void);
void Car_Task_OledShow_50ms(void);
void Car_Task_DataScope_50ms(void);//回传数据给手机
void Car_Task_LedBlink_500ms(void);//测试FreeRTOS,看程序是否运行正常
//接收MPU6050数据
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
if(GPIO_Pin == Mpu6050_Int_Pin)
{
__HAL_GPIO_EXTI_CLEAR_IT(Mpu6050_Int_Pin);
mpu_dmp_get_data(&pitchAngle, &gyroY, &gyroZ_turn);
}
}
//接收编码器数据
void GetEncode(void)
{
encode_right = (short)TIM2->CNT;
TIM2->CNT = 0;
encode_left = -((short)TIM3->CNT);
TIM3->CNT = 0;
}
//接收串口数据
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
HAL_UART_Receive_DMA(&huart2, buf, 12);
if(VerifyData(buf, 12) == 1)
{
GetPIDFromHC05(buf);
osSemaphoreRelease(HC05_BinarySemHandle);
}
}
放到main.h中
#include "myUsart.h"
#include "oled.h"
#include "oledfont.h"
#include "mpu6050.h"
#include "car_task.h"
#include "bluetooth_hc05.h"
#include "datarecive.h"
不需要去动,提供的是底层的支持,只要添加到工程中即可
CubeMX导出工程后,需要完成库文件的添加,相关函数要写入main.c和FreeRTOS.c中
需要添加的内容较多
①必须添加的
//此处必须要添加,能够让小车直立
void StartTask_PID_20ms(void *argument)
{
for(;;)
{
Car_Task_PID_Control_20ms();
osDelay(20);
}
}
②不影响直立的其他功能,可不添加
//让OLED显示数据,让手机能够控制小车
void StartTask_OLED_50ms(void *argument)
{
for(;;)
{
Car_Task_DataScope_50ms();
osDelay(50);
}
}
//看程序运行是否正常
void StartTask_LED_500ms(void *argument)
{
for(;;)
{
Car_Task_LedBlink_500ms();
osDelay(500);
}
}
仅列出用得到的函数,至于mpu_dmp_get_data如何获取MPU6050的DMP数据,在下文中还会有所展开,这里仅仅让大家明白程序的运行逻辑。
在查看直立环、速度环、转向环具体实现代码前,需要明确个数据
int Balance_Pwm_Cal(float Angle, float Mechanical_balance, float Gyro)
{
float Bias;
int balance;
Bias = Angle - Mechanical_balance;//角度差
balance = (Balance_Kp_Base + PIDFromHC05.BalanceKP) * Bias + (Balance_Kd_Base + PIDFromHC05.BalanceKD) * (Gyro / 10);
return balance;
}
int Velocity_Pwm_Cal(int encoder_left, int encoder_right)
{
static float Velocity, Encoder_Least, Encoder;
static float Encoder_Integral;
Encoder_Least = (encoder_left + encoder_right) - 0; //===获取最新速度偏差==测量速度(左右编码器之和)-目标速度(此处为零)
Encoder *= 0.8; //===一阶低通滤波器
Encoder += Encoder_Least * 0.2; //===一阶低通滤波器
Encoder_Integral += Encoder; //===积分出位移 积分时间:10ms
Encoder_Integral = Encoder_Integral - PIDFromHC05.Speed_Set * 2; //===接收遥控器数据,控制前进后退
if(Encoder_Integral > 10000) Encoder_Integral = 10000; //===积分限幅
if(Encoder_Integral < -10000) Encoder_Integral = -10000; //===积分限幅
Velocity = Encoder * (Speed_Kp_Base + PIDFromHC05.SpeedKP) + (Encoder_Integral / 100) * (Speed_Ki_Base + PIDFromHC05.SpeedKI); //===速度控制
if(pitchAngle < -40 || pitchAngle > 40) Encoder_Integral = 0; //===电机关闭后清除积分
return Velocity;
}
int Turn_Pwm_Cal(int encoder_left, int encoder_right, float gyro) //转向控制
{
static float Turn, Turn_Convert = 0.9, Turn_Count, Encoder_temp, Turn_Target;
float Turn_Amplitude = 44, Kd;
if(PIDFromHC05.Speed_Set != 0) //这一部分主要是根据旋转前的速度调整速度的起始速度,增加小车的适应性
{
if(++Turn_Count == 1)
Encoder_temp = myabs(encoder_left + encoder_right);
Turn_Convert = 55 / Encoder_temp;
if(Turn_Convert < 0.6)Turn_Convert = 0.6;
if(Turn_Convert > 3)Turn_Convert = 3;
}
else
{
Turn_Convert = 0.9;
Turn_Count = 0;
Encoder_temp = 0;
}
if(PIDFromHC05.Turn_Set != 0)
{
Turn_Target -= (PIDFromHC05.Turn_Set / 8) * Turn_Convert;
}
else
{
Turn_Target = 0;
}
if(Turn_Target > Turn_Amplitude) Turn_Target = Turn_Amplitude; //===转向 速度限幅
if(Turn_Target < -Turn_Amplitude) Turn_Target = -Turn_Amplitude;
if(PIDFromHC05.Speed_Set != 0) Kd = (Turn_Kd_Base+PIDFromHC05.TurnKD )/ 10.0;
else Kd = 0; //转向的时候取消陀螺仪的纠正 有点模糊PID的思想
Turn = -Turn_Target * (Turn_Kp_Base+PIDFromHC05.TurnKP) + (gyro / 100) * Kd; //===结合Z轴陀螺仪进行PD控制
return Turn;
}
HC05在我们用户眼中是透明的,我们不需要关注HC05是怎么手法数据的,我们只要将HC05波特率设置为与串口一致即可,剩下的工作全部是串口操作。
手机数据为12字节,包头为0xA5、包尾为0x5A、倒数第二位为校验位(为纯数据低八位的和)
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
if(HAL_OK == HAL_UART_Receive(&huart2, (uint8_t *)recv_buff, 12, 0xFFFF))
{
//可以自由对recv_buff进行操作
}
}
main()
{
HAL_UART_Receive_IT(&huart2, (uint8_t *)recv_buff, 12);//开启串口2中断
while (1)
{
//可以进行其他操作
}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
HAL_UART_Receive_IT(&huart2, (uint8_t *)recv_buff, 12);//开启串口2中断
if(VerifyData(recv_buff, 12))
{
GetPIDFromHC05(recv_buff);
printf("%d,%d,%d,%d\r\n", PIDFromHC05.BalanceKP, PIDFromHC05.BalanceKD, PIDFromHC05.SpeedKP, PIDFromHC05.SpeedKI);
}
}
串口只能一个字节一个字节发送数据,即只能发送uint8_t 类型数据,不能直接发送short/int/float等数据,必须先转换为uint8_t才可以
数据类型 | uint8 | short | int | float |
---|---|---|---|---|
占用字节 | 1 | 2 | 4 | 4 |
//target为需要转换的数据,buf是串口发送缓存数组,beg存放在数组中的位置
void Short2Byte(short *target,unsigned char *buf,unsigned char beg)
{
unsigned char *point;
point = (unsigned char*)target; //得到short的地址
buf[beg] = point[0];
buf[beg+1] = point[1];
}
具体用法为见下小节
void DataScopeGenerate(short gyro, short pwm, int encode, float angle)
{
uint8_t sumdata = 0;//放到校验位的数据,必须初始化为0,否则生成数据手机端无法解析
sendbuf[0] = 0xA5;//包头
Short2Byte(&gyro, sendbuf, 1);
Short2Byte(&pwm, sendbuf, 1 + lenshort);//lenshort=2
Int2Byte(&encode, sendbuf, 1 + lenshort + lenshort);
Float2Byte(&angle, sendbuf, 1 + lenshort + lenshort + lenint);//lenint=4
/*生成校验位数据,方法为发送缓存数组(待发送数据)之和的低8位置*/
for(uint8_t datak = 1; datak < SendDataLen - 2; datak++)
{
sumdata = sumdata + sendbuf[datak];
}
sendbuf[SendDataLen - 2] = sumdata;//校验位置
sendbuf[SendDataLen - 1] = 0x5A;//包尾
}
HAL_UART_Transmit_DMA(&huart2,sendbuf,SendDataLen);
/***********************************************************
*@fuction :VerifyData
*@brief :对数据进行校验
*@param :data为数据,dataBIT为位数
*@return :返回1验证通过
*@author :--
*@date :2023-07-19
***********************************************************/
char VerifyData(uint8_t *data, uint8_t dataBIT)
{
uint8_t sumdata = 0;
int datak;
if(data[0] == 0xa5 && data[dataBIT - 1] == 0x5a)//首先确保包头、包尾是对的
{
for(datak = 1; datak < 10; datak++)
{
sumdata = sumdata + data[datak];
}
if(sumdata == data[datak])//校验和也是对的
{
return 1;
}
return 0;
}
return 0;
}
struct HC05_PID_DATA
{
uint8_t LedStatus;
short BalanceKP;
short BalanceKD;
short SpeedKP;
short SpeedKI;
};
struct HC05_PID_DATA PIDFromHC05;
利用结构体取数据
void GetPIDFromHC05(uint8_t *data)
{
PIDFromHC05.LedStatus = data[1];
PIDFromHC05.BalanceKP = data[3] << 8 | data[2];
PIDFromHC05.BalanceKD = data[5] << 8 | data[4];
PIDFromHC05.SpeedKP = data[7] << 8 | data[6];
PIDFromHC05.SpeedKI = data[9] << 8 | data[8];
}
如要在freertos.c以外用二值信号量,需要添加以下内容
#include "cmsis_os.h"
extern osSemaphoreId_t myBinarySem_rxokHandle;
osSemaphoreRelease(myBinarySem_rxokHandle); //释放二值信号量
osSemaphoreAcquire(myBinarySem_rxokHandle,osWaitForever);//等待二值信号量,只有等到了才会往下运行
我对二值信号量的理解:我认为就是一个信号FLAG,中断来了就发个信号,这个信号相当于一个全局变量
OLED显示屏宽128像素,高64像素,即128x64,共8192个像素
为了清晰这里以32x16进行展示
显示过程:
c3将0x01写入红色方框1,按照低位在前的顺序,如图进行显示,而后一次是2/3/4/5
一个十六进制字节0x01可以确定8个像素的状态,由此可知屏幕 所需字节 = 128 ∗ 64 / 8 所需字节=128*64/8 所需字节=128∗64/8即1024字节.
使用Pctolcd软件生成,主要是取模方式为逐行列
/*对应显示原理中的32x16图片*/
const uint8_t bmp[] PROGMEM= {/*PROGMEM 可以不写,将数据放到程序存储空间
0xFE,0xFD,0xFC,0xFB,0xFA,0xF9,0xF8,0xF7,0xF6,0xF5,0xF4,0xF3,0xF2,0xF1,0xF0,0xEF,
0xEE,0xED,0xEC,0xEB,0xEA,0xE9,0xE8,0xE7,0xE6,0xE5,0xE4,0xE3,0xE2,0xE1,0xE0,0xDF,
0xDE,0xDD,0xDC,0xDB,0xDA,0xD9,0xD8,0xD7,0xD6,0xD5,0xD4,0xD3,0xD2,0xD1,0xD0,0xCF,
0xCE,0xCD,0xCC,0xCB,0xCA,0xC9,0xC8,0xC7,0xC6,0xC5,0xC4,0xC3,0xC2,0xC1,0xC0,0xBF,
};
OLED_Clear();//将显存数组清空
//写需要显示的内容,如OLED_ShowNum_Signal、OLED_ShowFLOAT
OLED_Refreash();//将显存数组数据发送至OLED
目前只能实现100HZ读取,没有实现网上广为流传的200HZ,只要调到200HZ就会死机。
MPU6050的INT脚不要忘接了!
我是直接在Embedded_MotionDriver_5.1库上进行修改的,官方示例是基于MSP430,要将msp430_i2c_write、msp430_i2c_read以及延时修改为STM32F401,其他不需要修改
#define i2c_write msp430_i2c_write
#define i2c_read msp430_i2c_read
#define delay_ms HAL_Delay
//已经修改为HAL_I2C_Mem_Write了
int msp430_i2c_write(uint16_t DevAddress, uint16_t MemAddress, uint16_t Size, uint8_t *pData)
{
if(HAL_I2C_Mem_Write(&hi2c1, DevAddress, MemAddress, I2C_MEMADD_SIZE_8BIT, pData, Size, 1000) == HAL_OK)
return 0;
else
return 1;
}
//已经修改为HAL_I2C_Mem_Read
int msp430_i2c_read(uint16_t DevAddress, uint16_t MemAddress, uint16_t Size, unsigned char *data)
{
HAL_I2C_Mem_Read(&hi2c1, DevAddress, MemAddress, I2C_MEMADD_SIZE_8BIT, data, Size, 1000);
return 0;
}
只需要写以下函数即可,参考官方Embedded_MotionDriver_5.1库写
#define DEFAULT_MPU_HZ (100)//采样频率
int mpu_dmp_init(void);//初始化
int run_self_test(void);
int mpu_dmp_get_data(float *pitch,short *gyroy,short *gyroz);
void mpu6050_init(void);
在car_task.c/Car_Init函数中
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);//开启PWM
引脚 | STBY | PWMA | AIN1 | AIN2 | AO1 | AO2 |
---|---|---|---|---|---|---|
功能 | 使能引脚,高电平有效 | PWMA | 与AIN2电平相反 | 与AIN1电平相反 | 输出1 | 输出2 |
以驱动左轮为例子
if(moto1 < 0)
{
moto1 = -moto1-Dead_Left_Motor_PWM;
HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
}
else
{
moto1 = moto1+Dead_Left_Motor_PWM;
HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
}
htim1.Instance->CCR4 = moto1;
这是STM32定时器自带的功能
short是数据格式强制转换,如果是正转CNT从0开始加、反转CNT从65535开始减,强制转换为short后,如果是正转CNT从0开始加、反转CNT从0开始减,方向直接在符号上体现了,不需要再去读取方向寄存器。
//左右轮编码器数据
void GetEncode(void)
{
encode_right = (short)TIM2->CNT;
TIM2->CNT = 0;
encode_left = -((short)TIM3->CNT);
TIM3->CNT = 0;
}
红色胶带底下是MPU6050,防止小车抖动导致模块松动后,DMP不准
完整电路图、程序下载:
链接:https://pan.baidu.com/s/1FCOt4zJ2VVpz19rcQ-R1Vg
提取码:6666
至此,我们的平衡小车已经全部完成,祝大家学习进步!