前俩篇博客记录了平衡小车的控制流程和硬件设计，本篇博客将讲解平衡小车的部分驱动以及核心控制算法。
1、电机驱动
motor.h
#ifndef __MOTOR_H
#define __MOTOR_H
#include "sys.h"
#include <stm32f10x.h>
#define PWM_LEFT TIM1->CCR1
#define PWM_RIGHT TIM1->CCR4
#define AIN1 PBout(13)
#define AIN2 PBout(12)
#define BIN1 PBout(14)
#define BIN2 PBout(15)
void MOTOR_PWM_Init(u16 arr,u16 psc);
void MOTOR_IO_Init(void);
#endif
motor.c
#include "motor.h"
#include "delay.h"
//电机PWM初始化
//arr：自动重装值
//psc：时钟预分频数
//定时器1 通道1，通道4
void MOTOR_PWM_Init(u16 arr,u16 psc)
{
RCC->APB2ENR|=1<<11; //使能TIM1时钟
RCC->APB2ENR|=1<<2; //使能PORTA时钟
RCC->APB2ENR|=1<<3; //使能PORTB时钟
//电机PWM引脚使能
GPIOA->CRH&=0XFFFF0FF0; //PA8、11输出
GPIOA->CRH|=0X0000B00B; //复用功能输出
TIM1->ARR=arr; //设定计数器自动重装值
TIM1->PSC=psc; //预分频器不分频
TIM1->CCMR2|=6<<12; //CH4 PWM1模式
TIM1->CCMR1|=6<<4; //CH1 PWM1模式
TIM1->CCMR2|=1<<11; //CH4预装载使能
TIM1->CCMR1|=1<<3; //CH1预装载使能
TIM1->CCER|=1<<12; //CH4输出使能
TIM1->CCER|=1<<0; //CH1输出使能
TIM1->BDTR |= 1<<15; //TIM1必须要这句话才能输出PWM
TIM1->CR1=0x8000; //ARPE使能
TIM1->CR1|=0x01; //使能定时器1
}
//电机IO初始化
//PB12、13、14、15
//AIN1->B13 AIN2->B12
//BIN1->B14 BIN2->B15
void MOTOR_IO_Init(void)
{
RCC->APB2ENR|=1<<3; //使能PORTB时钟
GPIOB->CRH&=0X0000FFFF; //PB12、13、14、15输出
GPIOB->CRH|=0X33330000; //推挽功能输出
}
我把电机的驱动分了俩个部分，一部分是TIM1输出的俩路PWM的初始化，另一部分是控制电机转动方向的IO口的初始化，很常规的，就不细说了。
2、编码器驱动
编码器是平衡小车中至关重要的部分，来获取电机的转速，STM32的定时器有编码器模式，通过硬件四分频来读取编码器，使用起来也是简单方便的，比自己写输入捕获简单的多，每个编码器需要一个定时器，我这里只用了TIM2和TIM4。
encoder.h
#ifndef __ENCODER_H
#define __ENCODER_H
#include <sys.h>
#define ENCODER_TIM_PERIOD (u16)(65535) //不可大于65535 因为F103的定时器是16位的。
void Encoder_Init_TIM2(void);
void Encoder_Init_TIM4(void);
int Read_Encoder(u8 TIMX);
#endif
encoder.c
#include "encoder.h"
/**************************************************************************
函数功能：把TIM2初始化为编码器接口模式（左编码器）
入口参数：无
返回 值：无
**************************************************************************/
void Encoder_Init_TIM2(void)
{
RCC->APB1ENR|=1<<0; //TIM2时钟使能
RCC->APB2ENR|=1<<2; //使能PORTA时钟
GPIOA->CRL&=0XFFFFFF00;//PA0 PA1
GPIOA->CRL|=0X00000044;//浮空输入
/* 把定时器初始化为编码器模式 */
TIM2->PSC = 0x0;//预分频器
TIM2->ARR = ENCODER_TIM_PERIOD-1;//设定计数器自动重装值
TIM2->CCMR1 |= 1<<0; //输入模式，IC1FP1映射到TI1上
TIM2->CCMR1 |= 1<<8; //输入模式，IC2FP2映射到TI2上
TIM2->CCER |= 0<<1; //IC1不反向
TIM2->CCER |= 0<<5; //IC2不反向
TIM2->SMCR |= 3<<0; //SMS='011' 所有的输入均在上升沿和下降沿有效
TIM2->CR1 |= 0x01; //CEN=1，使能定时器
}
/**************************************************************************
函数功能：把TIM4初始化为编码器接口模式（右编码器）
入口参数：无
返回 值：无
**************************************************************************/
void Encoder_Init_TIM4(void)
{
RCC->APB1ENR|=1<<2; //TIM4时钟使能
RCC->APB2ENR|=1<<3; //使能PORTb时钟
GPIOB->CRL&=0X00FFFFFF;//PB6 PB7
GPIOB->CRL|=0X44000000;//浮空输入
/* 把定时器初始化为编码器模式 */
TIM4->PSC = 0x0;//预分频器
TIM4->ARR = ENCODER_TIM_PERIOD-1;//设定计数器自动重装值
TIM4->CCMR1 |= 1<<0; //输入模式，IC1FP1映射到TI1上
TIM4->CCMR1 |= 1<<8; //输入模式，IC2FP2映射到TI2上
TIM4->CCER |= 0<<1; //IC1不反向
TIM4->CCER |= 0<<5; //IC2不反向
TIM4->SMCR |= 3<<0; //SMS='011' 所有的输入均在上升沿和下降沿有效
TIM4->CR1 |= 0x01; //CEN=1，使能定时器
}
/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回 值：速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0;break;
case 4: Encoder_TIM= (short)TIM4 -> CNT; TIM4 -> CNT=0;break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
3、ADC初始化以及获取电压值
因为STM32的ADC输入电压最高为3.3V，所以我搭了一个分压电路来采集4分频以后的电压值，然后再通过计算来得出真实电压值。
adc.h
#ifndef __ADC_H
#define __ADC_H
#include "sys.h"
#define Battery_Ch 4
void Adc_Init(void);
u16 Get_Adc(u8 ch);
int Get_battery_volt(void);
#endif
adc.c
#include "adc.h"
/**************************************************************************
函数功能：ACD初始化电池电压检测
入口参数：无
返回 值：无
**************************************************************************/
void Adc_Init(void)
{
//先初始化IO口
RCC->APB2ENR|=1<<2; //使能PORTA口时钟
GPIOA->CRL&=0XFFF0FFFF;//PA4 anolog输入
RCC->APB2ENR|=1<<9; //ADC1时钟使能
RCC->APB2RSTR|=1<<9; //ADC1复位
RCC->APB2RSTR&=~(1<<9);//复位结束
RCC->CFGR&=~(3<<14); //分频因子清零
//SYSCLK/DIV2=12M ADC时钟设置为12M,ADC最大时钟不能超过14M!
//否则将导致ADC准确度下降!
RCC->CFGR|=2<<14;
ADC1->CR1&=0XF0FFFF; //工作模式清零
ADC1->CR1|=0<<16; //独立工作模式
ADC1->CR1&=~(1<<8); //非扫描模式
ADC1->CR2&=~(1<<1); //单次转换模式
ADC1->CR2&=~(7<<17);
ADC1->CR2|=7<<17; //软件控制转换
ADC1->CR2|=1<<20; //使用用外部触发(SWSTART)!!! 必须使用一个事件来触发
ADC1->CR2&=~(1<<11); //右对齐
ADC1->SQR1&=~(0XF<<20);
ADC1->SQR1&=0<<20; //1个转换在规则序列中 也就是只转换规则序列1
//设置通道4的采样时间
ADC1->SMPR2&=0XFFF0FFFF; //采样时间清空
ADC1->SMPR2|=7<<12; // 239.5周期,提高采样时间可以提高精确度
ADC1->CR2|=1<<0; //开启AD转换器
ADC1->CR2|=1<<3; //使能复位校准
while(ADC1->CR2&1<<3); //等待校准结束
//该位由软件设置并由硬件清除。在校准寄存器被初始化后该位将被清除。
ADC1->CR2|=1<<2; //开启AD校准
while(ADC1->CR2&1<<2); //等待校准结束
}
/**************************************************************************
函数功能：AD采样
入口参数：ADC1 的通道
返回 值：AD转换结果
**************************************************************************/
u16 Get_Adc(u8 ch)
{
//设置转换序列
ADC1->SQR3&=0XFFFFFFE0;//规则序列1 通道ch
ADC1->SQR3|=ch;
ADC1->CR2|=1<<22; //启动规则转换通道
while(!(ADC1->SR&1<<1));//等待转换结束
return ADC1->DR; //返回adc值
}
/**************************************************************************
函数功能：读取电池电压
入口参数：无
返回 值：电池电压 单位MV
**************************************************************************/
int Get_battery_volt(void)
{
int Volt;//电池电压
Volt=Get_Adc(Battery_Ch)*3.3*4.0*100/1.0/4096; //电阻分压，四倍
if(Volt>1260)Volt=1260;
return Volt;
}
4、中断初始化
exit.h
#ifndef __EXTI_H
#define __EXIT_H
#include "sys.h"
#define INT PBin(5) //PB5连接到MPU6050的中断引脚
void EXTI_Init(void); //外部中断初始化
#endif
exit.c
#include "exti.h"
/**************************************************************************
函数功能：外部中断初始化
入口参数：无
返回 值：无
**************************************************************************/
void EXTI_Init(void)
{
RCC->APB2ENR|=1<<3; //使能PORTB时钟
GPIOB->CRL&=0XFF0FFFFF;
GPIOB->CRL|=0X00800000;//PB5上拉输入
GPIOB->ODR|=1<<5; //PB5上拉
Ex_NVIC_Config(GPIO_B,5,FTIR); //下降沿触发
MY_NVIC_Init(2,1,EXTI9_5_IRQn,2); //抢占2，子优先级1，组2
}
5、控制程序
/**************************************************************************
函数功能：所有的控制代码都在这里面
5ms定时中断由MPU6050的INT引脚触发
严格保证采样和数据处理的时间同步
**************************************************************************/
int EXTI9_5_IRQHandler(void)
{
if(PBin(5)==0)
{
EXTI->PR=1<<5; //清除LINE5上的中断标志位
Flag_Target=!Flag_Target;
if(delay_flag==1)
{
if(++delay_50==10) //给主函数提供50ms的精确延时 以刷新OLED及发送数据
{
delay_50=0;
delay_flag=0;
}
}
if(Flag_Target==1) //5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
{
Get_Angle(Way_Angle); //===更新姿态
if(++Flash_R_Count==150&&Angle_Balance>30)
Flash_Read(); //读取FLASH PID值
Voltage_Temp=Get_battery_volt(); //读取电池电压
Voltage_Count++;
Voltage_All+=Voltage_Temp;
if(Voltage_Count==100) //100次采样计算平均值
{
Voltage=Voltage_All/100;
Voltage_Count=0;
Voltage_All=0;
}
return 0;
} //10ms控制一次，为了保证M法测速的时间基准，首先读取编码器数据
Encoder_Left=Read_Encoder(2); //===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反，保证输出极性一致
Encoder_Right=Read_Encoder(4); //===读取编码器的值
Get_Angle(Way_Angle); //===更新姿态
HCER_04_Get_Length(); //===读取距离值
if(Bi_zhang==0)
Led_Flash(100); //===LED闪烁;常规模式 1s改变一次指示灯的状态
if(Bi_zhang==1)
Led_Flash(0); //===LED闪烁;避障模式 指示灯常亮
Balance_Pwm =balance(Angle_Balance,Gyro_Balance); //===平衡PID控制
Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); //===速度环PID控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===转向环PID控制
//printf("Gyro_Turn: %f Turn_Pwm: %d\r\n",Gyro_Turn,Turn_Pwm);
Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; //===计算左轮电机最终PWM
Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; //===计算右轮电机最终PWM
Xianfu_Pwm(); //===PWM限幅
if(Turn_Off(Angle_Balance,Voltage)==0) //===如果不存在异常
Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器
if(Bi_zhang==0)
Led_Flash(100); //===LED闪烁;常规模式 1s改变一次指示灯的状态
else
Led_Flash(0); //===LED闪烁;避障模式 指示灯常亮
if(Pick_Up(Acceleration_Z,Angle_Balance,Encoder_Left,Encoder_Right))//===检查是否小车被那起
Flag_Stop=1; //===如果被拿起就关闭电机
if(Put_Down(Angle_Balance,Encoder_Left,Encoder_Right)) //===检查是否小车被放下
Flag_Stop=0; //===如果被放下就启动电机
if(Turn_Off(Angle_Balance,Voltage)==0) //===如果不存在异常
Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器
}
return 0;
}
6、直立环
/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回 值：直立控制PWM
**************************************************************************/
int balance(float Angle,float Gyro)
{
float Bias,kp=410,kd=1.4;
int balance;
Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和机械相关
balance=kp*Bias+Gyro*kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
return balance;
}
7、速度环
/**************************************************************************
函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity，比如，改成60就比较慢了
入口参数：左轮编码器、右轮编码器
返回 值：速度控制PWM
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral,Target_Velocity;
float kp=-120,ki=-0.6;
//============遥控前进后退部分====================//
if(Bi_zhang!=0&&Flag_sudu==1) //进入避障模式，自动进入低速模式
Target_Velocity=55;
else
Target_Velocity=110;
if(Flag_Qian==1) //前进标志位置1
Movement=-Target_Velocity/Flag_sudu;
else if(Flag_Hou==1) //后退标志位置1
Movement=Target_Velocity/Flag_sudu;
else //停止
Movement=0;
if(Bi_zhang==1&&Flag_Left!=1&&Flag_Right!=1)
{
if(Distance<500)
Movement=Target_Velocity/Flag_sudu;
}
//=============速度PI控制器=======================//
Encoder_Least =(Encoder_Left+Encoder_Right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.8; //===一阶低通滤波器
Encoder += Encoder_Least*0.2; //===一阶低通滤波器
Encoder_Integral +=Encoder; //===积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退
if(Encoder_Integral>10000)
Encoder_Integral=10000; //===积分限幅
if(Encoder_Integral<-10000)
Encoder_Integral=-10000; //===积分限幅
Velocity=Encoder*kp+Encoder_Integral*ki; //===速度控制
if(Turn_Off(Angle_Balance,Voltage)==1||Flag_Stop==1) //电机关闭后清除积分
Encoder_Integral=0;
return Velocity;
}
8、转向环
/**************************************************************************
函数功能：转向控制 修改转向速度，请修改Turn_Amplitude即可
入口参数：左轮编码器、右轮编码器、Z轴陀螺仪
返回 值：转向控制PWM
**************************************************************************/
int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
float Turn_Amplitude=88/Flag_sudu,Kp=42,Kd=-0.7;
//=============遥控左右旋转部分=======================//
if(1==Flag_Left||1==Flag_Right) //这一部分主要是根据旋转前的速度调整速度的起始速度，增加小车的适应性
{
if(++Turn_Count==1)
Encoder_temp=myabs(encoder_left+encoder_right);
Turn_Convert=50/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(1==Flag_Left)
Turn_Target-=Turn_Convert;
else if(1==Flag_Right)
Turn_Target+=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(Flag_Qian==1||Flag_Hou==1)
Kd=-0.7;
else Kd=0; //转向的时候取消陀螺仪的纠正 有点模糊PID的思想
//=============转向PD控制器=======================//
Turn=-Turn_Target*Kp -gyro*Kd; //===结合Z轴陀螺仪进行PD控制
return Turn;
}
以上就是平衡小车项目中的吸血核心代码，注释也是写的很清楚的，其他的比如MPU6050，它的驱动是相当多的，我们不需要去完全理解它的所有代码，前期只要能读出6个数据（三个方向的角度及角加速度）就可以了，还有比如OLED、蓝牙、超声波的驱动都是很简单的，如果想要看的话可以去看我以前智能小车的博客，里面有详细的代码。
平衡小车的总结博客就结束了，平衡小车是我学了PID后做的第一个项目，在代码上借鉴了好多平衡小车之家的资料，在项目中也学到了好多东西，受益匪浅。
我平衡小车的演示视频再B站：链接点我