开始
本文总结了stm32平衡车的开发时所涉及的知识点。
硬件
核心板:stm32f103c8t6
OLED：用来指示AX，AY，AZ，BX，GY，GZ 目标角度，实际角度，以及PID输出
SCL：PB8
SDA：PB9
LED：核心板LED的PC13，用来指示PID调控的启停
Key：KEY2=PC14，KEY3=PC15，KEY12用来启停PID调控。
定时器：
Timer1的CH1(PA8),CH4(PA11)产生两路PWM
Timer2,Timer3编码器模式用来测速
Timer4产生1ms中断，中断程序中分频调控角度环，速度环，转向环，按键捕获
mpu6050：加速度计和陀螺仪获取Y轴俯仰角，软件IIC驱动
SCL（PB11）
SDA（PB10）
电机接线：
左电机
红：电机电源正 AO1
黑：编码器电源- 3V3负极
黄：编码器相线 PA0 TIM2
绿：编码器相线 PA1 TIM2
蓝：编码器电源+ 3V3正极
白：电机电源负 AO2
右电机：
红：电机电源正 BO1
黑：编码器电源- 3V3负极
黄：编码器相线 PA6 TIM3
绿：编码器相线 PA7 TIM3
蓝：编码器电源+ 3V3正极
白：电机电源负 BO2
蓝牙模块HC-05：配合微信小程序查看目标值与实际值波形，滑杆实时调参，摇杆控制
RX：USART2_TX PA2
TX：USART2_RX PA3
tb6612模块:
STBY 高电平(+3.3V)
PWMA:TIM1_CH1 左电机PWM ：PA8
PWMB:TIM1_CH4 右电机PWM ：PA11
VM 12V 接电池
VCC 3.3V tb6612模块的供电
GND 和单片机共地
左侧电机方向控制：
AIN1：pb14
AIN2: pb15
右侧电机方向控制：
BIN1:pb12
BIN2:pb13软件
MPU6050
由于MPU6050获取数据的代码用时太长，导致中断重叠，需要对代码进行改造。
定时器中断重叠问题
中断里面要执行的代码时间超过了中断定时时间，也就是本次中断中的代码还没有执行完，下一次的中断又来了，这就是中断重叠。
定时时间到，定时器会置中断标志位为1，中断标志位交给中断控制器NVIC，NVIC会时刻查看中断标志位，如果中断标志位为1并且当前没有抢占优先级更高或者相等的中断函数执行，那这个中断函数就会被调用。对于上述中断重叠的问题，不会形成中断嵌套，因为优先级相同自己不会嵌套自己，那下一个中断什么时候被响应取决于什么时候清除中断标志位。即
c
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);如果出现中断重叠问题，并且在退出中断函数之前最后时刻才清除标志位，那第二个中断函数响应将会被忽略，即
c
void TIM4_IRQHandler(void)
{
if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
{
/*
所有要执行的代码
*/
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
}
}还是在中断重叠的情况下，如果我们在进入中断函数后立刻清除标志位，那么在本次中断退出后会立刻响应第二个中断，即
c
void TIM4_IRQHandler(void)
{
if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
/*
所有要执行的代码
*/
}
}看现象：
如果最后清除中断标志位，OLED刷新特别慢，因为中断中执行的代码太长了，影响了OLED的刷新。如果刚进中断就清除标志位，由于中断中要执行的代码超出了中断的定时时长，导致本次中断还没执行完，下次中断标志位又置1，单片机得不停地执行中断的代码，主程序中刷新OLED的代码压根没时间执行，导致OLED黑屏。
MPU6050驱动寄存器设置：
c
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x07);//调整采样率为1000HZ，即获取角度的速率
MPU6050_WriteReg(MPU6050_CONFIG, 0x00);//不使用MPU6050自带的低通滤波器虽然输出会产生噪声，但延迟低mpu6050代码升级
去掉SDA，SCL的IO翻转延时Delay_US（10）测一下中断用时900多us占用中断百分之90的时间
c
//从原来一次读取一个寄存器的值到现在一次读取14个连续的寄存器的值
void MPU6050_ReadRegs(uint8_t RegAddress, uint8_t *DataArray, uint8_t Count)
{
uint8_t i;
MyI2C_Start();
MyI2C_SendByte(MPU6050_ADDRESS);
MyI2C_ReceiveAck();
MyI2C_SendByte(RegAddress);
MyI2C_ReceiveAck();
MyI2C_Start();
MyI2C_SendByte(MPU6050_ADDRESS | 0x01);
MyI2C_ReceiveAck();
for (i = 0; i < Count; i ++)
{
DataArray[i] = MyI2C_ReceiveByte();
if (i < Count - 1)
{
MyI2C_SendAck(0);
}
else
{
MyI2C_SendAck(1);//最后一个字节才应答
}
}
MyI2C_Stop();
}
void MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ,
int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
uint8_t Data[14];
MPU6050_ReadRegs(MPU6050_ACCEL_XOUT_H, Data, 14);
*AccX = (Data[0] << 8) | Data[1];
*AccY = (Data[2] << 8) | Data[3];
*AccZ = (Data[4] << 8) | Data[5];
*GyroX = (Data[8] << 8) | Data[9];
*GyroY = (Data[10] << 8) | Data[11];
*GyroZ = (Data[12] << 8) | Data[13];
}中断函数继续改造
c
void TIM4_IRQHandler(void)
{
if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
{
//进来先清标志位
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
/*
要执行的所有代码
*/
//再判断一下中断是否重叠
if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
{
//重叠了就提示一下
TimerErrorFlag = 1;
//再次清楚标志位
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
}
//TimerCount 代表中断执行时长
TimerCount = TIM_GetCounter(TIM4);
}
}编码器
编码器电路接口：
1,6号引脚是电机的电源正负极这里是12V，2,5引脚是编码器的正负极这里是3.3V，3,4引脚是正交编码器的两个信号输出。
电源指示灯电路与霍尔传感器电路，霍尔传感器通过EA ,EB 输出。
这是电机尾部黑色圆形磁铁的内部构造，它被均分成22个部分，N极S极交错排列，当霍尔遇到S输出低电平，遇到N极输出高电平。
工作原理：
电机转动带动圆形磁铁转动，：正转A相超前B相90度，反转A相滞后B相90度，旋转速度越快波形频率越高。磁铁旋转一周AB相各输出11个脉冲。由于编码器设置的是AB相上升沿下降沿都计数所以磁铁旋转一周总共计数（2（上升沿下降沿）+2）11=44个。同时由于这是一个减速电机（前面有减速箱），加速比为1/9.3,所以输出轴转一圈电机要转9.3圈，传感器应该要计数9.444=408.
总结：电机输出轴转一圈，编码器计数值加或减408，所以Location/408就是输出轴圈数，Location/408*360就是输出轴总共转了多少角度。Speed值与定时周期有关，现在定时周期是40ms,那么Speed的单位就是：边沿数/40ms,Speed/408就是 转/40ms，Speed/408/0.04 就是 转/秒。
stm32的编码器
每个高级定时器和通用定时器都拥有一个编码器接口，如果定时器配置成编码器接口模式，其他功能不能使用了。编码器接口通过接收正交信号自动控制CNT增加或减小，从而指示编码器的旋转方向速度以及当前位置。输入通道就是此定时器的输入捕获通道1和通道2.
计次功能就是将AB相所有的边沿作为计数时钟，出现边沿就自增或自减。
方向判断：就是出现某一个边沿时看另一相的状态，就是上图所示。
测速度：每隔一段时间取一次计次，得到的就是速度。
c
void Encoder_Init(void)
{
/*TIM3*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 65536 - 1; //计数这里一般设置为最大值
TIM_TimeBaseInitStructure.TIM_Prescaler = 1 - 1; //PSC
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStructure);
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
//调整IM_ICPolarity_Rising, TIM_ICPolarity_Falling可以改变编码器的极性
TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_Cmd(TIM3, ENABLE);
/*TIM2*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 65536 - 1; //ARR
TIM_TimeBaseInitStructure.TIM_Prescaler = 1 - 1; //PSC
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM2, &TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM2, &TIM_ICInitStructure);
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
TIM_Cmd(TIM2, ENABLE);
}
int16_t Encoder_Get(uint8_t n)
{
int16_t Temp;
if (n == 1)
{
Temp = TIM_GetCounter(TIM2);
TIM_SetCounter(TIM2, 0);
return Temp;
}
else if (n == 2)
{
Temp = TIM_GetCounter(TIM3);
TIM_SetCounter(TIM3, 0);
return Temp;
}
return 0;
}俯仰角与互补滤波
加速度计和陀螺仪测角度的优缺点
加速度计静态时测得准，但受振动，运动影响大，陀螺仪相对角度测得准，但有零漂问题。
加速度计测角度
XOZ平面就是绕Y轴旋转，使用蓝牙串口打印AXAYAZ的波形
c
//AngelAcc =atan(AX/1.0/AZ)/3.14159*180;//弧度值转角度值只支持-90到+90
AngelAcc =atan2(AX,AZ)/3.14159*180;//支持180 到 -180== 受运动加速度影响，加减速时，加速度计测到的角度会有偏移，振动颠簸都会有影响==
陀螺仪测角度
每隔一段时间测一下角度的变化量，在初始角度的基础上就可以得到当前角度值。
c
//陀螺仪满量程范围设置的是2000，这里需要换算得到度/秒
AngleGyro=AngleGyro+GY/32768.0*2000*0.001;//初始角度加角度的变化量（积分）
互补滤波
使用一阶惯性单元，以陀螺仪为主，加速度计为辅助，参考加速度计角度，来修正陀螺仪角度的漂移。
陀螺仪角度在逐渐靠近加速度计的角度。离得越远陀螺仪角度靠近加速度计角度的速度就越快。
加速度计角度作为一个锚点，陀螺仪角度漂移时受弹簧（互补滤波）牵制在一个范围内，不会因为振动对加速度计的影响又能限制陀螺仪的零漂。
c
int16_t AX, AY, AZ, GX, GY, GZ;
uint8_t TimerErrorFlag;
uint16_t TimerCount;
float AngleAcc;//加速度计得到的俯仰角
float AngleGyro;//陀螺仪测角度
float Angle;//互补滤波后的角度
void TIM4_IRQHandler(void)
{
if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
GY-=30;//减去陀螺仪零漂，去除加速度计与陀螺仪的稳态误差（先给GY=0，看零漂多少减多少）
//AngleAcc =atan(AX/1.0/AZ)/3.14159*180;//弧度值转角度值只支持-90到+90
AngleAcc =-atan2(AX,AZ)/3.14159*180;//支持180 到 -180,负号调整极性，前倾为正角度增大
//陀螺仪满量程范围设置的是2000，这里需要换算得到度/秒
//AngleGyro=AngleGyro+GY/32768.0*2000*0.001;//初始角度加角度的变化量（积分）
AngleGyro=Angle+GY/32768.0*2000*0.001;//互补滤波后的陀螺仪角度要在滤波后的角度基础上累加
float Alpha =0.001;
Angle=Alpha*AngleAcc+(1-Alpha)*AngleGyro;
if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
{
TimerErrorFlag = 1;
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
}
TimerCount = TIM_GetCounter(TIM4);
}
}平衡车控制结构
使用三个PID控制环路：
角度环：内环 ，即直立环或平衡环，通过传感器的到小车角度进行PID控制维持平衡车的直立，输出值直接作用与平均PWM，角度环输入的目标值大于0小车前进，小于0小车后退。
速度环：外环，通过控制角度换的额目标俯仰角使得小车的实际行进速度时钟稳定在用户设定的目标行进速度，目标速度是用户设定的行进速度，实际速度是电机编码器测量得到的平均速度，速度环的输出控制角度环的输入也就是小车的倾斜角来间接控制速度。实际速度大于目标速度是速度环输出就让倾斜角小一点，目标速度小于实际速度时，速度环的输出就让倾斜角大一点。
转向环：即使两个轮子给相同的PWM，小车也不一定能走直线，所以加入转向环。转向环有两个输入，目标值即使用户输入的转弯速度和实际值即是编码器测得的差分速度，转向环的输出差分PWM等于0，小车走直线。
安全措施
按键设置标志位控制启停，关闭PID调控后要要将三个环的PID相关参数都清零，防止失控。
倾斜角度大于50或者小于-50时也要关闭PID调控
c
/*指示灯*/
if (RunFlag) {LED_ON();} else {LED_OFF();}
/*按键*/
if (Key_Check(KEY_1, KEY_SINGLE)) //K1按下，启动/停止
{
if (RunFlag == 0)
{
PID_Init(&AnglePID);
PID_Init(&SpeedPID);
PID_Init(&TurnPID);
Angle = AngleAcc_Filter;
RunFlag = 1;
}
else
{
RunFlag = 0;
}
BlueSerial_Printf("RunFlag=%d\r\n", RunFlag);
}运动控制思路
一个变量控制前后运动（平均PWM），另一个变量控制左右差分转向（差分PWM）
平均PWM是左右轮PWM的平均值，差分PWM是左右轮PWM的差值
差分PWM为正，小车右转，左轮速度大于右轮速度小车右转，差分速度为正。
使用蓝牙串口进行调参前，先确定极性是否正确。发现小车总是朝一个方向移动，说明中心角给的不对，这可能与小车结构不对称或者mpu6050模块的安装角度有关，这时需要校准一下中心角度。从左右两边用手推小车，小车脱离手指开始倒下角度是多少就给AngleACC加多少。对应代码的这一句：
c
AngleAcc =-atan2(AX,AZ)/3.14159*180;
AngleAcc+=4.5;//中心值校准直立环PID控制
调参前要确定极性，即小车前倾两个轮子朝前转，后倾朝后转。可以先通过调节AO1，AO2，BO1,BO2接线使两个轮子转向一致，代码端可以调整输出值正负来调整轮子前进与后退。
蓝牙串口的一些设置
c
//
if (BlueSerial_RxFlag == 1)
{
char *Tag = strtok(BlueSerial_RxPacket, ",");
if (strcmp(Tag, "key") == 0)
{
char *Name = strtok(NULL, ",");
char *Action = strtok(NULL, ",");
}
else if (strcmp(Tag, "slider") == 0)
{
char *Name = strtok(NULL, ",");
char *Value = strtok(NULL, ",");
if (strcmp(Name, "AngleKp") == 0){
AnglePID.Kp=atof(Value);
}else if(strcmp(Name,"AngleKi")==0){
AnglePID.Ki=atof(Value);
}else if(strcmp(Name,"AngleKd")==0){
AnglePID.Kd=atof(Value);
}else if (strcmp(Name, "SpeedKp") == 0){
SpeedPID.Kp=atof(Value);
}else if(strcmp(Name,"SpeedKi")==0){
SpeedPID.Ki=atof(Value);
}else if(strcmp(Name,"SpeedKd")==0){
SpeedPID.Kd=atof(Value);
}else if (strcmp(Name, "TurnKp") == 0){
TurnPID.Kp=atof(Value);
}else if(strcmp(Name,"TurnKi")==0){
TurnPID.Ki=atof(Value);
}else if(strcmp(Name,"TurnKd")==0){
TurnPID.Kd=atof(Value);
}
}
else if (strcmp(Tag, "joystick") == 0)
{
int8_t LH = atoi(strtok(NULL, ","));
int8_t LV = atoi(strtok(NULL, ","));
int8_t RH = atoi(strtok(NULL, ","));
int8_t RV = atoi(strtok(NULL, ","));
AnglePID.Target=LV/10;//摇杆控制小车前倾角度在0-10度之内
DifPWM=RH/2;//差分PWM
}
BlueSerial_RxFlag = 0;
}
c
int16_t AX, AY, AZ, GX, GY, GZ;
uint8_t TimerErrorFlag;
uint16_t TimerCount;
float AngleAcc;//加速度计得到的俯仰角
float AngleGyro;//陀螺仪测角度
float Angle;//互补滤波后的角度
uint8_t KeyNum;
uint8_t RunFlag;//PID运行标志位
int16_t LeftPWM,RightPWM;
int16_t AvePWM,DifPWM;
PID_t AnglePID={
.Kp=4.3,
.Ki=0.23,
.Kd=3.5,
.OutMax=100,
.OutMin=-100,
};
void TIM4_IRQHandler(void)
{
static uint16_t count0=0;
if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
Key_Tick();
count0++;//直立环10ms调控一次
if(count0>=10){
count0=0;
MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
GY-=30;//消除陀螺仪零漂
AngleAcc =-atan2(AX,AZ)/3.14159*180;
AngleAcc+=4.5;//中心值校准
AngleGyro=Angle+GY/32768.0*2000*0.01;//原来是0.001，即1ms计算一次，现在角速度积分周期变为10ms
float Alpha =0.01;//互补滤波受调控周期影响，原来1ms，现在10ms需要增大，
Angle=Alpha*AngleAcc+(1-Alpha)*AngleGyro;
if(Angle>50||Angle<-50){
RunFlag=0;//小车倒地停止运行
}
if(RunFlag){
AnglePID.Actual=Angle;
PID_Update(&AnglePID);
AvePWM=AnglePID.Out;
LeftPWM=AvePWM+DifPWM/2;
RightPWM=AvePWM-DifPWM/2;
if(LeftPWM>100){
LeftPWM=100;
}else if(LeftPWM<-100){
LeftPWM=-100;
}
if(RightPWM>100){
RightPWM=100;
}else if(RightPWM<-100){
RightPWM=-100;
}
Motor_SetPWM(1,LeftPWM);
Motor_SetPWM(2,RightPWM);
}else{
Motor_SetPWM(1,0);
Motor_SetPWM(2,0);
}
}
if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
{
TimerErrorFlag = 1;
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
}
TimerCount = TIM_GetCounter(TIM4);
}
}速度环PID调参
极性确定：直立环调好的参数不变，给速度环KP为1，KIKD为0，项前推小车速度环正确的极性应该是控制目标角度为负，因为前进时陀螺仪输出为正，速度环目标值为负才能抑制小车速度使小车停止，同理向后推小车，速度环目标倾斜角为正。
速度环的输出单位是 :转/秒,给到角度环的输入，而角度环的单位是度，这里有疑问？
KP，KI，KD都是有单位的，以速度环为例，KP的单位是 ： 度/转/秒 乘以角度环的误差 转/秒 得到的输出值就是度。
c
else if (strcmp(Tag, "joystick") == 0){
int8_t LH = atoi(strtok(NULL, ","));
int8_t LV = atoi(strtok(NULL, ","));
int8_t RH = atoi(strtok(NULL, ","));
int8_t RV = atoi(strtok(NULL, ","));
SpeedPID.Target=LV/25.0;//控制速度环的输入 最大4转每秒
TurnPID.Target=RH/25.0;
}
c
count1++;
if(count1>=50){//速度环调控周期控制
count1=0;
LeftSpeed=Encoder_Get(1)/44.0/0.05/9.27666;//得到电机转速
RightSpeed=Encoder_Get(2)/44.0/0.05/9.27666;
AveSpeed=LeftSpeed+RightSpeed;
DifSpeed=LeftSpeed-RightSpeed;
if(RunFlag){
SpeedPID.Actual=AveSpeed;
PID_Update(&SpeedPID);
AnglePID.Target=-SpeedPID.Out;//速度环的输出作为直立环的输入，注意这里极性
TurnPID.Actual=DifSpeed;//转向环
PID_Update(&TurnPID);
DifPWM=TurnPID.Out;
}
}转向环PID
极性判断：直立环和速度环PID参数保持调整好的不便，给转向环KP为1，用手转动小车，转向环应该输出一个阻值转向的力，这种极性才是对的。通过OLED数值也可以判断极性，左转小车时实际值应该输出一个负值，因为（DifSpeed=LeftSpeed-RightSpeed;），输出值应该是个正值因为输出值要使左右轮速度趋向于一致使得DifPWM为0.
能走直线就说明有效果。