【请认准：OpenSir开源达人】
开源STM32自平衡小车
平衡小车开源资料网盘链接: 平衡小车百度网盘资料链接，点击进入 【哔站视频一键三连后，评论区留言邮箱获取提取码(3天内发至邮箱)】
哔站播放视频链接如下： 链接: 【开源STM32自平衡小车】哔站播放视频效果，点击进行播放
ps：如有不愿意进行自主diy的小伙伴们，我们也为其提供有推荐的套件 链接: STM32自平衡小车开源整车套件，点击即可进行查看
文章目录
【请认准：OpenSir开源达人】一、硬件篇（附淘宝器件链接，也可自行选购）1.STM32F103C8T6最小系统板（核心板）2.MPU6050姿态传感器（六轴）3.TB6612电机驱动（双通道）4.0.96寸OLED显示屏5.蓝牙无线通信模块6.电源降压模块（双电压输出）7.超声波测距模块8.动力锂电池（双节18650）9.小车橡胶车轮（65mm）10.STLINK代码仿真下载器
二、软件篇1.主函数代码2.systick滴答定时器中断任务3.滴答定时器5ms中断配置4.调试串口发送数据至上位机5.ADC采用DMA方式进行数据采集6.陀螺仪数据校准7.超声波测距控制8.欧拉角姿态解析9.PID控制器10.蓝牙数据包接收并解析
三、原理图
提示：以下是本篇文章正文内容
一、硬件篇（附淘宝器件链接，也可自行选购）
1.STM32F103C8T6最小系统板（核心板）
推荐链接: STM32F103C8T6最小系统核心板，点击进入!
2.MPU6050姿态传感器（六轴）
推荐链接: MPU6050姿态传感器模块，点击进入! 传感器资料链接:
https://pan.baidu.com/s/1dNDqcp76L9QdM7iSZYfz_A 提取码: 4eum
https://pan.baidu.com/s/13ZoMZ-lpAtDRhey4lKEcFQ 提取码: p7fr
3.TB6612电机驱动（双通道）
推荐链接: TB6612电机驱动（双通道），点击进入!
4.0.96寸OLED显示屏
推荐链接: 0.96寸OLED显示屏模块（128x64），点击进入! 模块资料链接：
https://pan.baidu.com/s/108smkVOLg-23cAnr37ZeGQ 密码：91t7
https://pan.baidu.com/s/1Lr8lw_6vt_VdM1UWe9CGsA 密码：p0un
5.蓝牙无线通信模块
推荐链接: JDY-31蓝牙无线通信模块，点击进入! DY-31带底板资料：
https://pan.baidu.com/s/16qyiOO05whOXqYtBVBGs2Q
6.电源降压模块（双电压输出）
推荐链接: 5V和3.3V双通道输出降压模块，点击进入!（咨询客服）
7.超声波测距模块
推荐链接: HC-SR04超声波测距模块，点击进入! HC-SR04资料下载：
http://pan.baidu.com/s/1sjHKDI1
HC-SR04单芯片版本资料下载接：
https://pan.baidu.com/s/1sSah9PvLBrmbA7So-6YcSw 提取码: qq35
8.动力锂电池（双节18650）
推荐链接: 3.7V 18650锂电池，点击进入!
9.小车橡胶车轮（65mm）
10.STLINK代码仿真下载器
推荐链接: STLINK V2代码烧录器，点击进入!
STLINK资料网盘链接:
https://pan.baidu.com/s/1gxzJeDe7CJaCCl4pGnwdNQ 提取码: an2m
WIN10系统驱动解决方案
https://pan.baidu.com/s/1OQosxeUMnCe5CZkwKRKhOA 提取码: h9ii
二、软件篇
点击文章顶部 B站视频链接 或者 点击此处 一键三连+关注，留言邮箱即可获取本小车Keil源代码文件
1.主函数代码
代码如下（main.c）：
/**********************************************************************
版权所有：源动力科技
淘 宝：https://1024tech.taobao.com/
文 件 名：mian.c
版 本：V21.01.30
摘 要：
***********************************************************************/
#include "stm32f10x.h"
#include "systick.h"
#include "led.h"
#include "iic.h"
#include "mpu6050.h"
#include "timer.h"
#include "nvic.h"
#include "imu.h"
#include "pwm.h"
#include "flash.h"
#include "pid.h"
#include "usart1.h"
#include "bluetooth.h"
#include "key.h"
#include "ps2.h"
#include "oled.h"
#include "adc.h"
#include "ultrasonic.h"
#include "imath.h"
#include "Infrared.h"
/*
* 使能SWD， 失能JTAG
* PB3,PB4,PA15作为普通IO使用
* MCU复位后，PA13/14/15 & PB3/4默认配置为JTAG功能
*/
static void _SWJ_Config(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
}
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //优先级组别2
_SWJ_Config(); //使能SWD，失能JTAG
usart1_init(115200); //串口1配置初始化
printf("usart1 is ok\r\n");
BluetoothUsart_init(9600); //蓝牙串口初始化
pwm_init(); //pwm初始
driver_pin_init(); //电机驱动配置初始化
Encoder_A_init(); //编码器A初始化
Encoder_B_init(); //编码器B初始化
tdelay_ms(100);
IIC_Init(); //IIC端口配置
mpu6050_init(); //mpu6050初始化
while(1) //mpu6050在位检测
{
uint8_t mpuId;
mpuId = get_mpu_id(); //读取mpu6050的id
if(mpuId==0x68||mpuId==0x98) //判断mpu6050的ID是否正确
break;
tdelay_ms(50);
}
get_iir_factor(&Mpu.att_acc_factor,0.005f,25); //姿态解算时加速度低通系数
OLED_Init(); //OLED显示屏端口初始化
adc_init(); //ADC配置初始化
ReadCalData(); //读取校准后的陀螺仪零偏数据
tdelay_ms(100);
timer_init(); //定0时器初始化
HCSR04_Init(); //超声波端口初始化
systick_init(); //滴答定时器初始化
NVIC_config(); //中断配置初始化
while(1)
{
if(softTimes[0] == 0) //100ms
{
softTimes[0] = 20;
}
if(softTimes[1] == 0) //20ms
{
softTimes[1] = 4;
UltrasonicWave_StartMeasure(); //超声波发出起始高电平
ParseBluetoothMessage(USARTxBlueTooth_RX_DATA, &BluetoothParseMsg); //蓝牙数据包解析
}
if(softTimes[2] == 0) //100ms
{
softTimes[2] = 20;
voltage_detection(); //低电压检测
OledShowDatas();
}
}
}
2.systick滴答定时器中断任务
代码如下（SysTick_Handler() ）：
/**
* @brief This function handles SysTick Handler.
* @param None
* @retval None
*/
void SysTick_Handler(void)
{
softTimesCountdown(); //软件定时倒计时
UltrasonicCheck(); //超声波在位检测
get_gyro_raw(); //陀螺仪数据
get_deg_s(&gyro_raw_f,&Mpu.deg_s); //陀螺仪原始数据转为度为单位的速率
get_rad_s(&gyro_raw_f,&Mpu.rad_s); //陀螺仪原始数据转为弧度为单位的速率
get_acc_raw(); //加速度数据
acc_iir_lpf(&acc_raw_f,&acc_att_lpf,Mpu.att_acc_factor); //姿态解算时加速度低通滤波
get_acc_g(&acc_att_lpf,&Mpu.acc_g);
//姿态解算
mahony_update(Mpu.rad_s.x,Mpu.rad_s.y,Mpu.rad_s.z,Mpu.acc_g.x,Mpu.acc_g.y,Mpu.acc_g.z);
Matrix_ready(); //姿态解算相关矩阵更新
encoder_val_a = read_encoder(ENCODER_A); //A编码器值读取
encoder_manage(&encoder_val_a); //编码器值处理
encoder_val_b = read_encoder(ENCODER_B); //B编码器值读取
encoder_manage(&encoder_val_b); //编码器值处理
ctr.pwm[0] = ctr_bal(att.rol,Mpu.deg_s.y); //角度直立平衡控制器
ctr.pwm[1] = ctr_vel(encoder_val_a,-encoder_val_b); //速度控制器
ctr.pwm[2] = ctr_turn(encoder_val_a,-encoder_val_b,Mpu.deg_s.z); //转向控制器
ctr.out[0] = ctr.pwm[0] + ctr.pwm[1] + ctr.pwm[2]; //电机1匹配输出
ctr.out[1] = ctr.pwm[0] + ctr.pwm[1] - ctr.pwm[2]; //电机2匹配输出
i_limit(&(ctr.out[0]),AMPLITUDE); //输出限幅
i_limit(&(ctr.out[1]),AMPLITUDE); //输出限幅
dir_config(&(ctr.out[0]),&(ctr.out[1])); //根据正负设置方向
_ctr_out(); //控制器输出
ANO_DMA_SEND_DATA(); //地面站波形显示
gyro_cal(&gyro_raw_cal); //陀螺仪零偏校准
}
3.滴答定时器5ms中断配置
代码如下（ systick_init ）：
/*
* 函数名：systick_init
* 描述 ：系统滴答定时器配置初始化
* 输入 ：
* 返回 ：
*/
void systick_init(void)
{
SystemCoreClockUpdate();
//时钟频率：72Mhz ， 每秒可以计数72000000次，一次的时间则为：1/72000（ms），10ms需要的次数：10/(1/72000) = 720000 -> 5ms需要的次数则为：360000
if (SysTick_Config(SystemCoreClock / 200)) //1000 -> 1ms
{
/* Capture error */
while (1);
}
SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk; //使能
}
4.调试串口发送数据至上位机
代码如下（ ANO_DMA_SEND_DATA ）：
vvoid ANO_DMA_SEND_DATA(void)
{
static uint8_t ANO_debug_cnt = 0;
ANO_debug_cnt++;
if(ANO_debug_cnt==1)
{
ANO_DT_Send_Status();
}
else if(ANO_debug_cnt==2)
{
ANO_DT_Send_Senser((int16_t)acc_raw.x,(int16_t)acc_raw.y,(int16_t)acc_raw.z,
(int16_t)gyro_raw.x,(int16_t)gyro_raw.y,(int16_t)gyro_raw.z,
(int16_t)0,(int16_t)0,(int16_t)0);
}
else if(ANO_debug_cnt==3)
{
ANO_DT_Send_RCData(1100,1200,1300,1400,1500,1600,1700,1800,1900,1100);
}
else if(ANO_debug_cnt==4)
{
ANO_DT_Send_Power(13.52,57.63);
}
else if(ANO_debug_cnt==5)
{
ANO_DT_Send_User(0,0,0,0,0,
encoder_val_a, encoder_val_b,ctr.pwm[0],ctr.pwm[1],acc_raw_f.z,
0,0,0,0,0);
}
else if(ANO_debug_cnt==6)
{
if(f.send_pid1)
{
f.send_pid1 = 0;
//向上位机发送内环速度环PID参数值
ANO_DT_Send_PID(1, vel.kp*0.1f,
vel.ki,
vel.kd,
1.5f,
2.5f,
1.5f,
2.5f,
1.5f,
2.5f);
}
if(f.send_pid2)
{
f.send_pid2 = 0;
//向上位机发送外环角度环PID参数值
ANO_DT_Send_PID(2, bal.kp*0.1f,
bal.ki,
bal.kd,
2.6f,
1.5f,
2.4f,
1.1f,
2.2f,
1.1f);
}
if(CalGyro.success==1) //返回校准成功信息给上位机
{
CalGyro.success = 0;
ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);
}
else if(CalGyro.success==2) //反馈校准失败信息给上位机
{
CalGyro.success = 0;
ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);
}
}
else if(ANO_debug_cnt==7)
{
ANO_debug_cnt = 0;
}
}
5.ADC采用DMA方式进行数据采集
代码如下：
/**********************************************************************
版权所有：源动力科技
淘 宝：https://1024tech.taobao.com/
文 件 名：adc.c
版 本：V21.01.30
摘 要：
***********************************************************************/
#include "adc.h"
_ADC_VALUE bat = {0 ,0 };
uint16_t adc_value[1];
/* 端口配置初始化 */
void adc_gpio_init(void)
{
GPIO_InitTypeDef GPIO_initStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
GPIO_initStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_initStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOA,&GPIO_initStructure);
}
/* adc配置 */
void adc_config(void)
{
ADC_InitTypeDef ADC_initStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1,ENABLE);
ADC_initStructure.ADC_ContinuousConvMode = ENABLE; //连续转换
ADC_initStructure.ADC_DataAlign = ADC_DataAlign_Right; //数据右对齐
ADC_initStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; //软件触发
ADC_initStructure.ADC_Mode = ADC_Mode_Independent; //独立工作模式
ADC_initStructure.ADC_NbrOfChannel = 1; //顺序进行规则转换的ADC通道的数目 1-16
ADC_initStructure.ADC_ScanConvMode = DISABLE; //扫描模式：单次模式
ADC_Init(ADC1,&ADC_initStructure);
ADC_Cmd(ADC1,ENABLE);
ADC_DMACmd(ADC1,ENABLE);
RCC_ADCCLKConfig(RCC_PCLK2_Div8); //ADC时钟分频
//通道配置，采样时间设置
ADC_RegularChannelConfig(ADC1,ADC_Channel_2,1,ADC_SampleTime_239Cycles5);
ADC_ResetCalibration(ADC1); //复位校准
while(ADC_GetCalibrationStatus(ADC1)); //等待
ADC_StartCalibration(ADC1); //启动校准
while(ADC_GetCalibrationStatus(ADC1)); //等待校准完成
ADC_SoftwareStartConvCmd(ADC1,ENABLE); //开启转换
}
void dma_config(void)
{
DMA_InitTypeDef DMA_initStructure;
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1,ENABLE);
DMA_initStructure.DMA_BufferSize = 1; //缓存的数据大小
DMA_initStructure.DMA_DIR = DMA_DIR_PeripheralSRC; //传输方向：外设->内存
DMA_initStructure.DMA_M2M = DMA_M2M_Disable;
DMA_initStructure.DMA_MemoryBaseAddr = (u32)adc_value; //内存基地址
DMA_initStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; //DMA传输的内存数据大小：半字为单位
DMA_initStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址自增
DMA_initStructure.DMA_Mode = DMA_Mode_Circular; //循环模式
DMA_initStructure.DMA_PeripheralBaseAddr = ((u32)&ADC1->DR); //外设地址
DMA_initStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; //DMA传输的外设数据大小：半字为单位
DMA_initStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外设地址不变
DMA_initStructure.DMA_Priority = DMA_Priority_Medium; //中等优先级
DMA_Init(DMA1_Channel1,&DMA_initStructure);
DMA_Cmd(DMA1_Channel1,ENABLE);
}
void adc_init(void)
{
adc_gpio_init();
adc_config();
dma_config();
}
/*
* 函数名：voltage_detection
* 描述 ：电源电压采集
* 输入 ：
* 返回 ：
*/
void voltage_detection(void)
{
bat.adc = adc_value[0];
bat.voltage = (float)(bat.adc)/4096.0f*3.3f*11.0f; //实际电路采用电阻10:1比例进行分压
if(bat.voltage<=7.0f)
{
bat.danger_flag = 1;
}
else
{
bat.danger_flag = 0;
}
}
6.陀螺仪数据校准
代码如下：
/*
* 函数名：gyro_cal
* 描述 ：陀螺仪零偏数据校准
* 输入 ：gyro_in原始的静止时陀螺仪数据首地址
* 返回 ：
*/
void gyro_cal(SI_F_XYZ *gyro_in)
{
if(CalGyro.flag==1 && 1)
{
if(CalGyro.i < GyroCalSumValue) //原始静止数据500次累加求取平均
{
CalGyro.offset.x += gyro_in->x;
CalGyro.offset.y += gyro_in->y;
CalGyro.offset.z += gyro_in->z;
CalGyro.i++;
}
else
{
CalGyro.i = 0;
CalGyro.OffsetFlashWrite.x = CalGyro.offset.x / GyroCalSumValue; //得到三轴的零偏
CalGyro.OffsetFlashWrite.y = CalGyro.offset.y / GyroCalSumValue; //得到三轴的零偏
CalGyro.OffsetFlashWrite.z = CalGyro.offset.z / GyroCalSumValue; //得到三轴的零偏
/* 将陀螺仪零偏写入flash */
FlashWriteNineFloat(SENSOR_CAL_ADDRESS, 0,0,0,
0,0,0,
CalGyro.OffsetFlashWrite.x,CalGyro.OffsetFlashWrite.y,CalGyro.OffsetFlashWrite.z);
/* 校准完数据之后立马读取出来进行使用 */
flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS, &CalGyro.None.x,
&CalGyro.None.y,
&CalGyro.None.z,
&CalGyro.None.x,
&CalGyro.None.y,
&CalGyro.None.z,
&CalGyro.OffsetFlashRead.x,
&CalGyro.OffsetFlashRead.y,
&CalGyro.OffsetFlashRead.z);
/* 判断是否正确读出 */
if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0)
{
_set_val(&gyro_offset,&CalGyro.OffsetFlashRead); //陀螺仪零偏设置
CalGyro.success = 1; //校准成功
_led.sta = 0;
}
else
{
CalGyro.success = 2; //校准失败
}
CalGyro.offset.x = 0;
CalGyro.offset.y = 0;
CalGyro.offset.z = 0;
CalGyro.flag = 0; //校准标志位清除
}
}
}
/*
* 函数名：ReadCalData
* 描述 ：陀螺仪零偏校准数据进行读取
* 输入 ：
* 返回 ：err
*/
int ReadCalData(void)
{
ErrorStatus err;
flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS, &CalGyro.None.x,
&CalGyro.None.y,
&CalGyro.None.z,
&CalGyro.None.x,
&CalGyro.None.y,
&CalGyro.None.z,
&CalGyro.OffsetFlashRead.x,
&CalGyro.OffsetFlashRead.y,
&CalGyro.OffsetFlashRead.z);
/* 判断是否正确读出 */
if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0)
{
_set_val(&gyro_offset, &CalGyro.OffsetFlashRead); //陀螺仪零偏设置
err = SUCCESS ;
}
else
{
err = ERROR;
}
return err;
}
7.超声波测距控制
代码如下：
float getDistance;
uint16_t EchoFlag = 0;
uint16_t disCounter;
void EXTI15_10_IRQHandler(void)
{
disCounter = 0;
tdelay_us(3); //延时10us
if(EXTI_GetITStatus(EXTI_Line13) != RESET)
{
EchoFlag = 1;
TIM_SetCounter(TIM1,0);
TIM_Cmd(TIM1, ENABLE);
while(GPIO_ReadInputDataBit(HCSR04_ECHO_PORT,HCSR04_ECHO_PIN)) //等待低电平
{
disCounter = TIM_GetCounter(TIM1);
if(disCounter>5000) //最远测量控制在5000计数值，换为距离为85cm
break;
}
TIM_Cmd(TIM1, DISABLE);
getDistance = TIM_GetCounter(TIM1)/1000000.0f * 340.0f / 2.0f * 100.0f; //us->s 声速340m/s 最终转为厘米cm
//计算公式为：距离=（高电平时间*340m/s）/2
// printf("%.3fcm %d\r\n",getDistance,TIM_GetCounter(TIM1));
EXTI_ClearITPendingBit(EXTI_Line13); //清除EXTI7线路挂起位
}
}
8.欧拉角姿态解析
代码如下（ mahony_update(float gx, float gy, float gz, float ax, float ay, float az) ）：
/**********************************************************************
版权所有：源动力科技
淘 宝：https://1024tech.taobao.com/
文 件 名：imu.c
版 本：V21.01.30
摘 要：
***********************************************************************/
#include "imu.h"
#include "imath.h"
#include "math.h"
#include "mpu6050.h"
#include "timer.h"
_Matrix Mat = {0};
_Attitude att = {0};
#define mahonyPERIOD 5.0f //姿态解算周期（ms）
#define kp 0.5f //proportional gain governs rate of convergence to accelerometer/magnetometer
#define ki 0.0001f //integral gain governs rate of convergenceof gyroscope biases
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //quaternion elements representing theestimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; //scaled integral error
/*
* 函数名：mahony_update
* 描述 ：姿态解算
* 输入 ：陀螺仪三轴数据（单位：弧度/秒），加速度三轴数据（单位：g）
* 返回 ：
*/
//Gyroscope units are radians/second, accelerometer units are irrelevant as the vector is normalised.
void mahony_update(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
if(ax*ay*az==0)
return;
//[ax,ay,az]是机体坐标系下加速度计测得的重力向量(竖直向下)
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
//VectorA = MatrixC * VectorB
//VectorA ：参考重力向量转到在机体下的值
//MatrixC ：地理坐标系转机体坐标系的旋转矩阵
//VectorB ：参考重力向量（0,0,1）
//[vx,vy,vz]是地理坐标系重力分向量[0,0,1]经过DCM旋转矩阵(C(n->b))计算得到的机体坐标系中的重力向量(竖直向下)
vx = Mat.DCM_T[0][2];
vy = Mat.DCM_T[1][2];
vz = Mat.DCM_T[2][2];
//机体坐标系下向量叉乘得到误差向量，误差e就是测量得到的vˉ和预测得到的 v^之间的相对旋转。这里的vˉ就是[ax,ay,az]’,v^就是[vx,vy,vz]’
//利用这个误差来修正DCM方向余弦矩阵(修正DCM矩阵中的四元素)，这个矩阵的作用就是将b系和n正确的转化直到重合。
//实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，
//但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
//两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积a×v=|a||v|sinθ,
//加速度计测量得到的重力向量和预测得到的机体重力向量已经经过单位化，因而他们的模是1，
//也就是说它们向量的叉积结果仅与sinθ有关，当角度很小时，叉积结果可以近似于角度成正比。
ex = ay*vz - az*vy;
ey = az*vx - ax*vz;
ez = ax*vy - ay*vx;
//对误差向量进行积分
exInt = exInt + ex*ki;
eyInt = eyInt + ey*ki;
ezInt = ezInt + ez*ki;
//姿态误差补偿到角速度上，修正角速度积分漂移，通过调节Kp、Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。
gx = gx + kp*ex + exInt;
gy = gy + kp*ey + eyInt;
gz = gz + kp*ez + ezInt;
//一阶龙格库塔法更新四元数
q0 = q0 + (-q1*gx - q2*gy - q3*gz)* mahonyPERIOD * 0.0005f;
q1 = q1 + ( q0*gx + q2*gz - q3*gy)* mahonyPERIOD * 0.0005f;
q2 = q2 + ( q0*gy - q1*gz + q3*gx)* mahonyPERIOD * 0.0005f;
q3 = q3 + ( q0*gz + q1*gy - q2*gx)* mahonyPERIOD * 0.0005f;
//把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
//四元素转欧拉角
att.pit = atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3) * rad_to_angle;
att.rol = asin(2.0f*(q0*q2 - q1*q3)) * rad_to_angle;
//z轴角速度积分的偏航角
att.yaw += Mpu.deg_s.z * mahonyPERIOD * 0.001f;
}
/*
* 函数名：rotation_matrix
* 描述 ：旋转矩阵：机体坐标系 -> 地理坐标系
* 输入 ：
* 返回 ：
*/
void rotation_matrix(void)
{
Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);
Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);
Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);
Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);
Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);
Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);
Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
}
/*
* 函数名：rotation_matrix_T
* 描述 ：旋转矩阵的转置矩阵：地理坐标系 -> 机体坐标系
* 输入 ：
* 返回 ：
*/
void rotation_matrix_T(void)
{
Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3);
Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2);
Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);
Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1);
Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);
Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);
Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
}
/*
* 函数名：Matrix_ready
* 描述 ：矩阵更新准备，为姿态解算使用
* 输入 ：
* 返回 ：
*/
void Matrix_ready(void)
{
rotation_matrix(); //旋转矩阵更新
rotation_matrix_T(); //旋转矩阵的逆矩阵更新
}
9.PID控制器
代码如下：
/*
ctr.bais[0]：角度控制器偏差存储
ctr.bais[1]：速度控制器偏差累计存储
ctr.bais[2]：转向控制器偏差存储
ctr.exp[0]：角度控制器期望角度
ctr.exp[1]：速度控制器期望速度
ctr.exp[2]：转向控制器期望角度
*/
/*
* 函数名：ctr_bal
* 描述 ：角度PD控制器
* 输入 ：angle当前角度，gyro当前角速度
* 返回 ：PID控制器输出
*/
int ctr_bal(float angle,float gyro)
{
ctr.exp[0] = 0; //期望角度
ctr.bais[0] = (float)(angle - ctr.exp[0]); //角度偏差
ctr.balance = bal.kp * ctr.bais[0] + gyro * bal.kd; //角度平衡控制
return ctr.balance;
}
int CarStepForwardOrBackward;
/*
* 函数名：ctr_vel
* 描述 ：速度PI控制器
* 输入 ：encoder_left编码器值A，encoder_right编码器值B
* 返回 ：PID控制器输出
*/
int ctr_vel(int encoder_left,int encoder_right)
{
static float encoder_cur,encoder_last;
CarStepForwardOrBackward = CarSpeedCtrlForwardOrBackward(1); //遥控器控制的前后方向速度
remote.ForwardOrBackward = remoteCarForwardOrBackward( (float)CarStepForwardOrBackward , MODE_BLUETEETH , UltraSuccess(BluetoothParseMsg.ModeRocker) ); //遥控器控制前后方向运动
encoder_last = ((encoder_left) + (encoder_right)) - 0; //速度误差
encoder_cur = encoder_cur * 0.8 + encoder_last * 0.2; //对偏差进行低通滤波
ctr.bais[1] += encoder_cur; //偏差累加和
ctr.bais[1] = ctr.bais[1] - remote.ForwardOrBackward; //遥控控制前后方向
if(ctr.bais[1] > 10000) ctr.bais[1] = 10000; //限幅
if(ctr.bais[1] <-10000) ctr.bais[1] =-10000;
ctr.velocity = encoder_cur * vel.kp + ctr.bais[1] * vel.ki; //速度控制
return ctr.velocity;
}
10.蓝牙数据包接收并解析
代码如下（ ParseBluetoothMessage(char *pInput ,uint16_t rcLens , BluetoothParse *blueParseMsg ) ）：
*
* 函数名：USART3_IRQHandler
* 描述 ：蓝牙串口接收中断处理函数
* 输入 ：
* 返回 ：
*/
void USART3_IRQHandler(void)
{
if(USART_GetITStatus(BLUETOOTH_USARTx, USART_IT_RXNE) != RESET)
{
if(BlueToothBufIndex > USARTxBlueTooth_RX_LEN)
BlueToothBufIndex = 0;
USARTxBlueTooth_RX_DATA[BlueToothBufIndex++] = USART_ReceiveData(BLUETOOTH_USARTx);
BlueToothNumbers ++;
USART_ClearITPendingBit(BLUETOOTH_USARTx, USART_IT_RXNE); //清除空闲中断标志
}
}
/*
* 函数名：ParseBluetoothMessage
* 描述 ：解析蓝牙app发送过来的数据包
* 输入 ：pInput收到的蓝牙遥控数据首地址 , blueParseMsg解析后保存的蓝牙数据有效信息
* 返回 ：err
*/
/*
* 协议形式：字符串
* 帧头：# 帧尾：*
* 例如：#9,1,1,2,2,1*
* 每位代表含义：9：数据长度(固定) 1：模式 1：关闭/开始状态 2：蓝牙摇杆X轴数据 2：蓝牙摇杆Y轴数据 1：校验值(累加和减去7)
*/
uint8_t ParseBluetoothMessage(char *pInput , BluetoothParse *blueParseMsg)
{
unsigned char plen,sum,check;
ErrorStatus err;
char *pdata = pInput;
while(( pdata-pInput ) < BlueToothBufIndex )
{
if(*pdata == '#')// #9,1,1,2,2,1* //接收到数据的包头#
{
plen = (unsigned char)atof(strtok(pdata+1, ",")); //解析长度length
if(plen == 9)
{
/* 将读出的数据进行累加校验 */
sum = (unsigned char)int_abs( ((int)atof(strtok(pdata+3, ",")) +
(int)atof(strtok(NULL, ",")) +
(int)atof(strtok(NULL, ",")) +
(int)atof(strtok(NULL, ","))) - 7 );
/* 读出累加校验数据 */
check = (unsigned char)atof(strtok(NULL, ",")) ; //累加校验数据
if(sum == check) //校验匹配成功才进行数据解析
{
blueParseMsg->ModeRocker = (unsigned char)atof(strtok(pdata+3, ",")); //模式数据
blueParseMsg->OpenCloseStatus = (unsigned char)atof(strtok(pdata+5, ",")); //关闭/开始状态数据
blueParseMsg->Xrocker = (unsigned char)atof(strtok(pdata+7, ",")); //摇杆X数据
blueParseMsg->Yrocker = (unsigned char)atof(strtok(pdata+9, ",")); //摇杆Y数据
err = SUCCESS;
}
else
{
err = ERROR;
}
}
else
{
err = ERROR;
}
}
else
{
err = ERROR;
}
pdata++;
}
BlueToothBufIndex = 0;
return err ;
}
三、原理图
【 PS：点击文章顶部 B站视频链接 或者 点击此处 一键三连+关注，留言邮箱即可获取本小车原理图纸文件。】