基于STM32的二轮自平衡小车
前言
近年来，移动机器人是目前科学领域比较活跃的领域之一，其应用范围越来越广泛，面临的环境也越来越复杂，这就要求机器人能够适应一些复杂的环境和任务。二轮自平衡机器人正是在这一背景下提出来的，对于制作此种类型的自平衡小车无疑对我stm32的学习有莫大的好处，一方面深入了解stm32以及学习串级PID的运用，另一方面也是对我近期学习的硬件方面知识一个检验。
本人为32小白水平有限，同时也是第一篇博客，如果有地方描述不清晰不正确，欢迎大佬指出一起讨论
原理介绍
-
参考资料
第七届全国大学生“飞思卡尔”杯智能汽车竞赛 读取码gveh
-
系统框图以及运动分析
系统框图
获取小车的速度和角度是实现小车自平衡的前提，数据在stm32的中断控制中配合PID算法使用，输出数值给PWM寄存器控制相应电机，从而实现平衡。遥控部分是控制转向，根据移动终端向蓝牙发送数据从而实现相应的运动。
我们可以将平衡小车的运动方式类比为手指上放置一根筷子，保持其直立状态，根据生活经验可知，当筷子向某个方向倒下时，我们眼睛观察到筷子倒下反馈给大脑，大脑控制手迅速向筷子倒下的方向运动，使其始终保持原有直立状态。同样，平衡小车道理相同，当传感器检测到小车有倒下的趋势时，控制轮子向相同方向运动，不同角度会输出不同速度即可保持平衡。
运动分析
平衡小车的控制可以分为直立控制，速度控制以及转向控制，即直立环、速度环和转向环，使小车能够保持平衡状态的是速度环以及直立环，通过传感器读取角度输出不同的PWM即不同的速度使其保持平衡。其中单独的直立环可以使小车保持直立，但如果受到外力的影响，那么不用多想，小车一定会倒下，将直立环和速度环串起来，那么当小车受到外力时，也能迅速平衡。
运动控制
小车的直立控制是通过负反馈控制的
高中时期大家对正反馈和负反馈都有所了解，这里不过多解释。直立控制实际上就是将小车控制在一定角度内，这个角度由小车的机械零点有关（你搭建的小车重心），对于机械零点的确定会在调试中说明，上文提到如果只有直立环不足以小车完全保持平衡，所以我们还需要加入速度环。
小车的速度控制是通过正反馈控制的
我们可以假设此时小车处于直立状态，如果小车想向前运动，那么小车必然要向前倾斜获取向前的加速度，但是向前倾斜轮子必然要向后转动，那么小车的速度会下降（轮子向后转动了）假如为负反馈那么小车必然会增加倾角，一直循环下去这样则会加速小车的倒下，所以在速度控制中反馈系统应该是正反馈。
转向控制
所谓转向环即当小车已经能保证基本的平衡时，通过蓝牙控制使小车改变方向的控制系统
- 定时器PWM输出以及编码器模式
PWM输出
脉冲宽度调制(PWM)，是英文“Pulse Width Modulation” 的缩写，简称脉宽调制，是利用微处理器的数字输出来对模拟电路进行控制的一种非常有效的技术。
当小车在运行过程中随着倾角的改变小车的速度也应该发生变化，如果只是单纯给予高低电平，只能以额定速度运动，显然不能达到我们想达到的程度，所以我们需要定时器输出PWM波，我们选用的主控模块只有4个定时器（1个高级3个通用）一个定时器有4个通道控制小车显然是足够的。当然如果你选择其他系列芯片会有多个定时器，选择也会更多
（注意基本定时器无法输出PWM脉冲）
其中如果用到高级定时器TIM1以及TIM8注意除了基础的配置还需要使能刹车和死区寄存器，以使能整个PWM输出，还需要使能高级定时器特有的寄存器。
TIM_CtrlPWMOutputs(TIM1,ENABLE); //高级定时器特有
编码器模式
上文系统框图可以看出，速度系统需要一个速度反馈，其他系统的反馈可以通过6050读取，那么此时小车的速度如何读取呢？这里就需要编码器读取此时的速度。编码器会引出AB相，通过STM32的编码器模式以特定的GPIO口连接编码器AB相，读取脉冲，从而读取速度。
通过查阅参考手册不难得知只有高级定时器以及通用定时器具有检测编码器的功能，基本定时器并不具备这种功能。所以我们可以配置定时器来读取编码器数据。
这里需要注意的是：编码器模式只有定时器的通道一和通道二可以使用，即只有TIMx_CH1 TIMx_CH2 可以使用
千万注意这一点，如果自己打板的话没注意这一点就可以重新打板了 /(ㄒoㄒ)/
参考手册如下：
选择编码器接口模式的方法是：如果计数器只在TI2的边沿计数，则置TIMx_SMCR寄存器中的SMS=001；如果只在TI1边沿计数，则置SMS=010；如果计数器同时在TI1和TI2边沿计数，则置SMS=011。
通过设置TIMx_CCER寄存器中的CC1P和CC2P位，可以选择TI1和TI2极性；如果需要，还可以对输入滤波器编程。
两个输入TI1和TI2被用来作为增量编码器的接口。参看下表：
假定计数器已经启动(TIMx_CR1寄存器中的CEN=’1’)，计数器由每次在TI1FP1或TI2FP2上的有效跳变驱动。 TI1FP1和TI2FP2是TI1和TI2在通过输入滤波器和极性控制后的信号；如果没有滤波和变相，则TI1FP1=TI1，TI2FP2=TI2。根据两个输入信号的跳变顺序，产生了计数脉冲和方向信号。依据两个输入信号的跳变顺序，计数器向上或向下计数，同时硬件对TIMx_CR1寄存器的DIR位进行相应的设置。不管计数器是依靠TI1计数、依靠TI2计数或者同时依靠TI1和TI2计数。在任一输入端(TI1或者TI2)的跳变都会重新计算DIR位。
编码器接口模式基本上相当于使用了一个带有方向选择的外部时钟。这意味着计数器只在0到TIMx_ARR寄存器的自动装载值之间连续计数(根据方向，或是0到ARR计数，或是ARR到0计数)。所以在开始计数之前必须配置TIMx_ARR；同样，捕获器、比较器、预分频器、触发输出特性等仍工作如常。在这个模式下，计数器依照增量编码器的速度和方向被自动的修改，因此计数器的内容始终指示着编码器的位置。计数方向与相连的传感器旋转的方向对应。
当定时器配置成编码器接口模式时，提供传感器当前位置的信息。使用第二个配置在捕获模式的定时器，可以测量两个编码器事件的间隔，获得动态的信息(速度，加速度，减速度)。指示机械零点的编码器输出可被用做此目的。根据两个事件间的间隔，可以按照固定的时间读出计数器。如果可能的话，你可以把计数器的值锁存到第三个输入捕获寄存器(捕获信号必须是周期的并且可以由另一个定时器产生)；也可以通过一个由实时时钟产生的DMA请求来读取它的值。
更详细请参考编码器配置原理
MPU-6050姿态解算
-
六轴传感器读取角度
想要小车直立起来由上文可知我们需要读取y轴的倾斜角度以及y轴的加速度，当然如果用到转向环还要读取z轴的角度。MPU-6050可读取3轴的角度以及3轴的加速度和芯片的温度。其中角度是直立环重要参数，角速度是速度环以及转向环的重要参数。
获得6050的数据我们可以通过移植官方的DMP库，通过DMP，就可以使用InvenSense公司提供的运动处理资料库，非常方便地实现姿态解算，也可以获得原始数据后使用卡尔曼滤波解算姿态。
需要注意的是AD0接口如果接地，则6050的IIC地址为0x68，如果接vcc，则6050的IIC地址0x69参考资料 MPU-6050
PID
-
PID算法
上边运动分析中已经提及小车的运动需要直立环速度环以及转向环。PID的学习包括理论知识以及调参的经验等。
参考资料 PID算法原理、调试经验和代码z2r8
模块选型
主控模块
主控模块我们使用STM32f103c8t6 ，这款模块有3个通用定时器，一个高级定时器，3路串口，2路IIC，72Mhz，对于我们的需求是绰绰有余的。
编码器
编码器电机我们使用的是平衡之家的一款mini电机，由于打出的板子体型较小，所以没有选择体积大的电机。
额定电压7.4V，当然也可以工作在12V环境下，编码器供电5v。
价格差不了多少，还是体积大一点的搭建出来的小车看起来大气
电机线1/6接在电机模块的A/BOUT处，电机线2/5接5v以及接地，编码器AB相接在预留出来的定时器接口只有通道1/2支持
电机驱动模块
电机驱动模块选择的是TB6612模块，VM需要12v供电，SYBT需要5v供电否则电机不会转动， 同时输出两路PWM，同时控制两个电机。
这里其实一开始选择是功能更强大的A4950，但是因为摔了一下电机模块就直接罢工（太便宜），所以在购置硬件时千万不要图便宜，买好的不买便宜的
降压模块
降压模块选择一块带数显的，方便观察电池电量，不多叙述。
MPU-6050模块
不多赘述，同样6050也不要图便宜（这一块可能就是因为质量问题，导致程序无法进入6050的外部中断，最后没办法放入主程序运行）
蓝牙通讯模块
转向环则是实现小车转弯的一环。通常利用上位机与平衡小车的交互来来控制转向，这里使用的是蓝牙模块HC-06，HC-05/HC-06 /SPP-C这些通用的型号都可以用，蓝牙模块通过串口通信来传输数据。
硬件准备
IO口分配
定时器TIM4预留给OLED 每10ms刷新一次数据
定时器TIM3用于测距通过OLED展示距离
另外注意编码器只能用通道1/2
| GPIO | 复用情况 | 连接外设 |
|---|---|---|
| PA2 | USART2_RX | 蓝牙 TX |
| PA3 | USART2_TX | 蓝牙 RX |
| PA0 | TIM2_CH1 | 电机A编码器A相 |
| PA1 | TIM2_CH2 | 电机A编码器B相 |
| PA6 | TIM3_CH1 | 电机B编码器B相 |
| PA7 | TIM3_CH2 | 电机B编码器A相 |
| PA8 | TIM1_CH1 | TB6612 PWMA |
| PA11 | TIM1_CH4 | TB6612 PWMB |
| PB1 | TIM3_CH3 | SR04 -> Trig |
| PB0 | TIM3_CH4 | SR04 -> Echo |
| PB5 | 无 | 6050外部中断 |
| PB6 | IIC1 SCL | 6050 SCL |
| PB7 | IIC1 SDA | 6050 SDA |
| PB10 | IIC2 SCL | OLED SCL |
| PB11 | IIC2 SDA | OLED SDA |
| PB12 | 无 | BIN2 |
| PB13 | 无 | BIN1 |
| PB14 | 无 | AIN1 |
| PB15 | 无 | AIN2 |
原理图绘制
原理图如图所示，供电接口采用的T型接口，电机等处加入100nf电容进行滤波。
PCB绘制
PCB未铺铜如图所示
铺铜以及添加泪滴效果
2D预览
实物图
硬件组装
3S电池搭建在最底层，中间放置降压模块，铜柱搭建最上方PCB，防止电机运动时的抖动影响系统的运行。
其实这里是有问题的，预留的OLED位置不够，导致只能放置在边上，PCB打孔处和降压模块冲突，整体排布也影响重心，需要调试机械中值。
实物图如下
硬件装配完成，下面我们开始软件编程
软件调试
程序是删减后的，并不完整 ，程序还是自己写学习才更有意义
配置编码器
#include "encoder.h"
void Encoder_TIM2_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化GPIO--PA0、PA1
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0 |GPIO_Pin_1;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=65535;
TIM_TimeBaseInitStruct.TIM_Prescaler=0;
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器模式 T1和T2的每个跳变沿均计数 TIM_ICPolarity_Rising：不反相。
/***********************
根据两个输入信号（TI1&TI2）的跳变顺序，产生了计数脉冲和方向信号。
依据两个输入信号的跳变顺序，计数器向上或向下计数，同时硬件对TIMx_CR1寄存器的DIR位进行相应的设置。
不管计数器是依靠TI1计数、依靠TI2计数或者同时依靠TI1和TI2计数。
在任一输入端(TI1或者TI2)的跳变都会重新计算DIR位。
【正反转】
正转：T1超前T2相位90度。
反转：T1滞后T2相位90度。
【模式】
TI1模式：在T1的所有边沿计数。
TI2模式：在T2的所有边沿计数。
TI12模式：在T1和T2的所有边沿计数。
*************************/
TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获
TIM_ICInitStruct.TIM_ICFilter=10;//滤波器
TIM_ICInit(TIM2,&TIM_ICInitStruct);
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位
TIM_SetCounter(TIM2,0);//清零定时器计数值
TIM_Cmd(TIM2,ENABLE);//开启定时器
}
/**********************
编码器
速度读取函数
入口参数：定时器
**********************/
int Read_Speed(int TIMx)
{
int value_1;
switch(TIMx)
{
case 2:value=(short)TIM_GetCounter(TIM2);
TIM_SetCounter(TIM2,0);
break;
//IF是定时器2，1.采集编码器的计数值并保存。2.将定时器的计数值清零。
case 3:value=(short)TIM_GetCounter(TIM3);
TIM_SetCounter(TIM3,0);
break;
default:value=0;
}
return value;
}
void TIM2_IRQHandler(void)
{
if(TIM_GetITStatus(TIM2,TIM_IT_Update)!=0)
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中断标志位
}
}
void TIM3_IRQHandler(void)
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)!=0)
{
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}
}
配置PWM输出
#include "pwm.h"
void PWM_Init_TIM1(u16 Psc,u16 Per)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;//初始化GPIO--PA8、PA11为复用推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 |GPIO_Pin_11;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=Per;
TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);
TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;//初始化输出比较 选择PWM1模式
TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;
TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;
TIM_OCInitStruct.TIM_Pulse=0;
TIM_OC1Init(TIM1,&TIM_OCInitStruct);
TIM_OC4Init(TIM1,&TIM_OCInitStruct);
TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器专属
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);
TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1预装载寄存器使能
TIM_Cmd(TIM1,ENABLE);//使能定时器。
}
配置电机
#include "motor.h"
/*电机初始化函数*/
void Motor_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//初始化GPIO--PB12、PB13、PB14、PB15为推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_12 |GPIO_Pin_13 |GPIO_Pin_14 |GPIO_Pin_15;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
}
int PWM_MAX = 3000;
int PWM_MIN = -3000;
/*限幅函数*/
void Limit(int *motoA,int *motoB)
{
if(*motoA>PWM_MAX)
*motoA=PWM_MAX;
else if(*motoA<PWM_MIN)
*motoA=PWM_MIN;
if(*motoB>PWM_MAX)
*motoB=PWM_MAX;
else if(*motoB<PWM_MIN)
*motoB=PWM_MIN;
}
/*绝对值函数*/
int abs(int p)
{
int q;
q=p>0?p:(-p);
return q;
}
/*赋值函数*/
/*入口参数：PID运算完成后的最终PWM值*/
void Load(int moto1,int moto2)//moto1=-200：反转200个脉冲
{
//1.研究正负号，对应正反转
if(moto1>0) Ain1=1,Ain2=0;//正转
else Ain1=0,Ain2=1;//反转
//2.研究PWM值
TIM_SetCompare1(TIM1,abs(moto1)); //等价直接赋值在->CRR寄存器
if(moto2>0) Bin1=0,Bin2=1;
else Bin1=1,Bin2=0;
TIM_SetCompare4(TIM1,abs(moto2));
}
char PWM_Zero=0,stop=0;
void Stop(float *Med_Jiaodu,float *Jiaodu)
{
if(GFP_abs(*Jiaodu-*Med_Jiaodu)>50)
{
Load(PWM_Zero,PWM_Zero);
}
}
配置MPU-6050
直接移植的DMP库，为软件IIC，所以只需要修改管脚以及宏定义即可
因为我这款6050模块无法进入中断，所以预留的PB5外部中断接口暂时没用上，所以外部中断的GPIO的初始化不再展示
void IIC_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //50M
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOB
}
/*不同的管脚对应的修改的地址不一样，这里需要注意*/
#define SDA_IN() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=8<<28;}
#define SDA_OUT() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=3<<28;}
#define IIC_SCL PBout(6) //SCL
#define IIC_SDA PBout(7) //SDA
#define READ_SDA PBin(7) //输入SDA
配置蓝牙
蓝牙预留USART2，在中断程序中执行转向程序
我们用的平衡之家的APP，不同的程序所对应的代码不同，都为16进制数，自己查询软件指令即可。
//GPIO初始化
void USART2_init(u32 bound)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);
//USART2_TX
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOC, &GPIO_InitStructure);
//USART2_RX
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOC, &GPIO_InitStructure);
//USART 初始化设置
USART_InitStructure.USART_BaudRate = bound;//一般设置为9600;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART2, &USART_InitStructure);
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);//开启中断
USART_Cmd(USART2, ENABLE); //使能串口
NVIC初始化
MY_NVIC_PriorityGroupConfig(2);
NVIC_InitStruct.NVIC_IRQChannel=USART2_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=0;
NVIC_InitStruct.NVIC_IRQChannelSubPriority=0;
NVIC_Init(&NVIC_InitStruct);
}
u8 Fore,Back,Left,Right;
void USART2_IRQHandler(void) //先尝试能不能进中断 不能再放外边
{
char Bluetooth_data;
if(USART_GetITStatus(USART2,USART_IT_RXNE)!=RESET)//接收中断标志位拉高
{
Bluetooth_data=USART_ReceiveData(USART2);//保存接收的数据
if(Bluetooth_data==0x5A) Fore=0,Back=0,Left=0,Right=0;//刹
else if(Bluetooth_data==0x41) Fore=1,Back=0,Left=0,Right=0;//前
else if(Bluetooth_data==0x45) Fore=0,Back=1,Left=0,Right=0;//后
//else if(Bluetooth_data==0x42) Fore=1,Back=0,Left=1,Right=0;//右前
else if(Bluetooth_data==0x47) Fore=0,Back=0,Left=1,Right=0;//左
else if(Bluetooth_data==0x43) Fore=0,Back=0,Left=0,Right=1;//右
else Fore=0,Back=0,Left=0,Right=0;//刹
}
}
//一个字符
void USART2_Send_Data(char data)
{
USART_SendData(USART2,data);
while(USART_GetFlagStatus(USART2,USART_FLAG_TC)!=1);
}
//一串字符
void USART2_Send_String(char *String)
{
u16 len,j;
len=strlen(String);
for(j=0;j<len;j++)
{
USART2_Send_Data(*String++);
}
}
if((Fore==0)&&(Back==0))
Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
if(Fore==1)
Target_Speed--;//前进1标志位拉高-->需要前进
if(Back==1)
Target_Speed++;//
/*左右*/
if((Left==0)&&(Right==0))
Turn_Speed=0;
if(Left==1)
Turn_Speed+=30; //左转
if(Right==1)
Turn_Speed-=30; //右转
/*转向约束*/
if((Left==0)&&(Right==0))Turn_Kd=-0.8;//若无左右转向指令，则开启转向约束
else if((Left==1)||(Right==1))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束
PID
PID控制分为直立环以及速度环，转向环大同小异不再展示
调参步骤：
-
确立机械中值。
***机械中值：***把平衡小车放在地面上，从前向后以及从后向前绕电机轴旋转平衡小车，两次的向另一边倒下的角度的中值，就是机械中值。
-
直立环（内环）—Kp极性、Kp大小。Kd极性、Kd大小。
Kp极性：
极性错误：小车往哪边倒，车轮就往反方向开，会使得小车加速倒下。
极性正确：小车往哪边倒，车轮就往哪边开，以保证小车有直立的趋势。
Kp大小：
Kp一直增加，直到出现大幅低频震荡。
Kd极性：
极性错误：拿起小车绕电机轴旋转，车轮反向转动，无跟随。
极性正确：拿起小车绕电机轴旋转，车轮同向转动，有跟随。
Kd大小：
Kd一直增加，直到出现高频震荡。
直立环调试完毕后，对所有确立的参数乘以0.6作为最终参数。
-
速度环（外环）——Kp&Ki极性、Kp&Ki大小。
在调试【速度环参数极性】时：需要去掉（注释掉）【直立环运算】
在调试【速度环参数大小】时：再次引入（取消注释）【直立环运算】
ki/kp线性关系Ki=(1/200)*Kp、仅调Kp即可。
Kp&Ki极性：*
极性错误：手动转动其中一个车轮，另一车轮会以同样速度反向旋转——典型负反馈。
极性正确：手动转动其中一个车轮，两个车伦会同向加速，直至电机最大速度——典型正反馈。
Kp&Ki大小：
增加Kp&Ki，直至：小车保持平衡的同时，速度接近于零。且回位效果较好。
对于PID具体调参 参考运动分析部分处
/********************
入口：期望角度、真实角度、真实角速度
出口：直立环输出
*********************/
int Vertical(float Med,float Angle,float gyro_y)
{
int PWM_out;
PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_y-0);
return PWM_out;
}
/*********************
速度环PI：Kp*Ek+Ki*Ek_S
// 入口 左电机速度, 右点击速度
*********************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
static int Encoder_S,EnC_Err_Lowout_last,PWM_out,Encoder_Err,EnC_Err_Lowout;
float a=0.7;
//1.计算速度偏差
Encoder_Err=((encoder_left+encoder_right)-Target);
//2.对速度偏差进行低通滤波
low_out=(1-a)*Ek+a*low_out_last;
EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;//使得波形更加平滑，滤除高频干扰，防止速度突变。
EnC_Err_Lowout_last=EnC_Err_Lowout;//防止速度过大的影响直立环的正常工作。
//3.对速度偏差积分，积分出位移
Encoder_S+=EnC_Err_Lowout;
//4.速度环控制输出计算
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
return PWM_out;
}
主程序初始化
我们主要实现的是小车直立，所以OLED以及传感器不再展示
定义变量
float Med_Angle=2; //机械中值。
float
Vertical_Kp=-125
, //直立环KP、KD 225 215* 220* 129 125** 115
Vertical_Kd=-0.765; //1.1 1.18 1.21* 1.25* 0.768 0.773** 0.764
float
Velocity_Kp=1.0, //速度环KP、KI 1.11 1.14** 1.16 1.18
Velocity_Ki=0.005; // Ki = 1/200 * Kp
int MOTO1=0,MOTO2=0; // 电机装载变量
float Pitch,Roll,Yaw; //角度信息,俯仰角,翻滚角,偏航角
short gyrox,gyroy,gyroz;//陀螺仪角速度
short aacx,aacy,aacz;//加速度
int Encoder_Left,Encoder_Right;//编码器数据(速度)
int Vertical_out,Velocity_out,Turn_out;
初始化
int main(void)
{
//char buf[16];
uart_init(115200);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
delay_init(); //=====延时函数初始化
LED_Init(); //=====LED初始化 程序灯
I2C_Configuration(); //硬件I2C初始化
OLED_Init(); //OLED初始化
Encoder_TIM2_Init(); //编码器
Encoder_TIM3_Init();
IIC_Init(); //=====软件IIC初始化 读取MPU6050数据
MPU6050_initialize(); //=====MPU6050初始化
DMP_Init(); //=====初始化DMP
PWM_Init_TIM1(0,7199); //72M/(1) / (7200) = 10kHZ
delay_ms(1000); //延时1s 解决小车上电轮子乱转的问题
Motor_Init();//电机
TIM3_Configuration(); //超声波拿来捕获的定时器
UltrasonicWave_Configuration(); //超声波
UltrasonicWave_StartMeasure(); //超声波
BASIC_TIM_Init(); //TIM4基本定时器 只定时 更新数据用
EXTI_Init(); //=====MPU6050 5ms定时中断初始化
控制部分
控制部分包括将PID的最终输出加载到电机上以及在串口上打印出读取的角度以及角速度
while(1)
{
int PWM_out;
Read_DMP();
printf("%d\r\n",(int)Roll);
printf("%d\r\n",(int)Yaw);
printf("%d\r\n",(int)Pitch);
printf("%d\r\n",gyrox);
printf("%d\r\n",gyroy);
printf("%d\r\n",gyroz);
gyrox = gyro [0];
gyroy = gyro [1];
gyroz = gyro [2];
aacx = accel[0];
aacy = accel[1];
aacz = accel[2];
//1、采集编码器数据&MPU6050角度信息。
Encoder_Left=-Read_Speed(2);
Encoder_Right=Read_Speed(3);
//2、将数据压入闭环控制中，计算出控制输出量。
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
Vertical_out=Vertical(Velocity_out+Med_Angle,Pitch,gyroy); //直立环
Turn_out=Turn(gyroz,Turn_Speed);
PWM_out=Vertical_out;//最终输出
//3、把控制输出量加载到电机上，完成最终的的控制。
MOTO1=PWM_out;//左电机
MOTO2=PWM_out;//右电机
Limit(&MOTO1,&MOTO2); //PWM限幅
Load(MOTO1,MOTO2); //加载到电机上。
Stop(&Med_Angle,&Pitch);//安全检测
后言
到这里小车已经基本实现直立以及外力影响下快速恢复，（当然是你的PID调试十分顺利情况下，一开始我是直接试数盲调，所以我的调试过程十分令人暴躁，学长建议看波形图才逐渐调试好）现在回想调试过程中还是暴露出很多问题，包括6050无法进入中断，PCB打板出来无法使用等等问题。总的来说这次项目的独立完成还是能学到很多东西，从最初的原理运动分析到模块选型以及PCB学习和绘制，当然这种单纯绘制电路板还是不够完美，继续学习，争取下次做项目能完成集成的PCB，当然不局限于EDA，现在也在学习AD.由于时间紧迫以及个人能力有限，有地方可能表述不是很清楚，欢迎大佬们指出，欢迎在评论区提出来。
那么平衡小车的硬件以及软件教程就到这里啦，我们下次再见！！