目录
一.所需材料
二.PID算法（简单说明）
直立环
速度环
串级PID
三.使用到的外设
1.定时器输出比较-PWM
2.定时器编码器模式
3.编码器读取速度
4.电机驱动函数
5.外部中断
四、小车
调试
一.所需材料
1.陀螺仪MPU6050--读取三轴的加速度和角速度，然而我们平衡车需要的不是加速度和角速度，而是需要实时的角度，即我们还需要进行姿态角转换，可以直接移植正点原子的DMP函数
2.TB6612电机驱动，l298N也可，我用的是TB6612，点击查看接线
3.OLED，用来显示姿态角，可以直接移植
4.12v锂电池和DCDC降压模块给电机和单片机供电
5.编码电机，点击查看接线
6.c8t6单片机
二.PID算法（简单说明）
P（比例）算法：Kp*误差。
可以看出，误差越大，P的输出就越大，所以P的作用是减小误差，但是只有P算法会发生超调现象，即在不受外力下可以看作简谐运动，一直震荡
I（积分）算法：Ki*误差的积分。
可以看出，只要存在误差（一般叫做稳态误差，比如小车在平衡位置偏左或右一点点，但是又不会倒下等等的情况下产生的角度误差）就会一直累加，累加累加的总误差就变的很大，滚雪球一样，越来越大，所以I可以消除稳态误差
D（微分）：Kd*误差的微分。
可以看出，两次误差之差反映的是系统的反应速度，响应越快，误差之差越大，D输出很大，反之很小，所以D算法会抑制过冲，消弱系统的反应速度。例如简谐运动，以最下点为期望点，小球从空中开始速度很小，后面越来越快，即可以理解为误差微分越来越大，且为负的，所以D输出就越来越大，力的方向与小球运动方向相反，若没有D算法，小球就会继续简谐运动，若此时有了D算法，小球的运动就会收到阻碍而令小球到达的高度降低，长此以往，小球就会静止在最低点
也可以这样看，在水中挥拳，挥的速度越大，受到的阻尼越大
直立环
PWM_OUT=Kp*误差+Kd*误差的微分
误差=真实角度-期望角度
误差的微分=角速度，姿态角转换得到
作用：让小车保持短时间的平衡，但是推一下就可能倒下
/*
直立环
PWM_OUT = Kp*角度偏差+Kd*角度偏差的微分
角度偏差求微分就是对角度偏差求导，即为角速度
gyro_Y：俯仰角的角速度
*/
int Vertical(float Expect_Angle,float Angle,float gyro_Y)
{
int PWM_out1=0;
PWM_out1 = Vertical_Kp*(Expect_Angle-Angle)+Vertical_Kd*(gyro_Y-0);
return PWM_out1;
}
/*速度环
PWM_OUT=Kp*误差+Ki*误差的积分
误差：期望速度-真实速度
误差的积分=误差累加
作用：消除稳态误差，加强系统的反应速度
极性问题：
速度环在平衡一类的基本上都是用正反馈，即积分误差，放大误差，先注释掉直立环，则极性正确的现象就是转动一下轮子，立刻就会加速到最大，因为我们速度理论值是0，我们转了一下，产生了误差，速度环不断放大误差，使速度环输出越大，就会让轮子不断加速到最大速度，若是负反馈，我们很难转动轮子
int Velocity(int Target,int left,int right)
{
static int Encoder_S,EnC_Err_Lowout_last,PWM_out3,Encoder_Err,EnC_Err_Lowout;
float a = 0.7;
Encoder_Err = left+right-Target;//速度偏差
//对速度偏差进行一阶低通滤波
EnC_Err_Lowout = a*EnC_Err_Lowout_last+(1-a)*Encoder_Err;
EnC_Err_Lowout_last=EnC_Err_Lowout;
//积分
Encoder_S+=EnC_Err_Lowout;
//限幅
Encoder_S=I_Limit(Encoder_S);
PWM_out3 = Velocity_Kp*EnC_Err_Lowout + Velocity_Ki*Encoder_S;
return PWM_out3;
}
int I_Limit(int Encoder_S)
{
if(Encoder_S>10000)Encoder_S=10000;
else if(Encoder_S<-10000)Encoder_S=-10000;
else
Encoder_S=Encoder_S;
return Encoder_S;
}
速度环必须滤波，因为速度会突变，突变的速度会对系统产生很大影响
滤波：编码器测到的数据（速度）是存在一些突变的，这些突变会导致系统的不稳定，所以我们要滤除这些突变，这就是滤波，类似于模电里面的“滤除电路中的高频信号”
一阶低通滤波：把权重给到上次滤波后的速度，另一个这次的速度为（1-a）倍，0.5<a<1
积分限幅：误差的积分在一直累加，若没有限幅，积分会越来越大，举个例子：小车倒下了，我们为了保护电机而关掉了电机，但是单片机的电源并没有关掉，误差一直在累加，很大很大了，当我们把小车放好并打开电机后，由于I的作用根本无法平衡。所以要限幅。
串级PID
串级，顾名思义，就是一个环的输出作为另一个环的输入，例如速度环的输出作为直立环的输入，或者直立环的输出作为速度环的输入
速度环输入：1.给定速度。2.速度反馈。
输出：角度值（直立环的期望速度输入）
直立环输入：1.给定角度（速度环输出）。2.角度反馈
输出：PWM（直接控制小车）
Vertical_out=Kp1*(Angle-Expect_Angle)+Kd* gyro_y
直立环输出=Kp1*（真实角度-期望角度+机械中值）+Kd*角度偏差的微分
Velocity_out =Kp2*(Encoder_ real- Encoder_ expect)+Ki*(Encoder_ real- Encoder_ expect)的积分
速度环输出=Kp2*（反馈编码器值-期望编码器值）+Ki*编码器偏差的积分
合并推导：Expect_Angle = Velocity_out
Vertical_out = Kp1*{ Angle-[ Kp2*(Encoder_ real- Encoder_ expect)+Ki*Σ(Encoder_ real- Encoder_ expect) ]}+Kd*gyro_y
=Kp1*真实角度+ Kd*角度偏差的微分-Kp1* [Kp2*编码器偏差- Ki *编码器偏差的积分]
三.使用到的外设
1.定时器输出比较-PWM
void PWM_Init_TIM1(uint16_t Psc,uint16_t Per)
{
//开时钟,AFIO
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;//复用推挽
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_11;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//不分频----滤波器的采样频率，可以由内部时钟直接提供，
//也可以由内部时钟加一个时钟分频而来，
//分频系数就是由TIM_ClockDivision决定
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数
TIM_TimeBaseInitStruct.TIM_Period = Per;//ARR的值
TIM_TimeBaseInitStruct.TIM_Prescaler = Psc;//PSC的值
//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器，这里不需要
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);
TIM_OCInitTypeDef TIM_OCInitStruct;
TIM_OCStructInit(&TIM_OCInitStruct);//初始化输出比较结构体
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;//输出比较模式
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High; //极性，有效电平为高电平
TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;//输出使能
TIM_OCInitStruct.TIM_Pulse = 0;//CCR
TIM_OC1Init(TIM1,&TIM_OCInitStruct);
TIM_OC4Init(TIM1,&TIM_OCInitStruct);
TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器特有的，必须使能
//使能ARR影子寄存器
TIM_ARRPreloadConfig(TIM1,ENABLE);
//使能输出比较预装载值寄存器
TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);
TIM_Cmd(TIM1,ENABLE);
}
2.定时器编码器模式
//配置编码器
//编码器1-PA0/PA1-TIM2
//编码器2-PB6/PB7-TIM4
void Encoder_TIM2_Init()
{
//开启GPIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
//开启定时器时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
//初始化GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_Init(GPIOA,&GPIO_InitStruct);
//初始化定时器
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//滤波分频系数
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数
TIM_TimeBaseInitStruct.TIM_Period = 65535;//ARR的值
TIM_TimeBaseInitStruct.TIM_Prescaler = 0;//PSC的值
//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器，这里不需要
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
//编码器特有的配置函数,配置模式
TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
//初始化输入捕获
TIM_ICInitTypeDef TIM_ICInitStruct;
TIM_ICStructInit(&TIM_ICInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下
TIM_ICInitStruct.TIM_ICFilter = 10;// 滤波器，范围是 0x0 and 0xF
TIM_ICInit(TIM2,&TIM_ICInitStruct);
//清除定时器溢出中断标志位
TIM_ClearFlag(TIM2,TIM_FLAG_Update);
//配置溢出中断
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);
//设置计数器初始值为0
TIM_SetCounter(TIM2,0);
//开启定时器
TIM_Cmd(TIM2,ENABLE);
}
void Encoder_TIM4_Init()
{
//开启GPIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
//开启定时器时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
//初始化GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_Init(GPIOB,&GPIO_InitStruct);
//初始化定时器
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//外部时钟滤波分频系数
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数
TIM_TimeBaseInitStruct.TIM_Period = 65535;//ARR的值
TIM_TimeBaseInitStruct.TIM_Prescaler = 0;//PSC的值
//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器，这里不需要
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);
//编码器特有的配置函数
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
//配置IC输入
TIM_ICInitTypeDef TIM_ICInitStruct;
TIM_ICStructInit(&TIM_ICInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下
TIM_ICInitStruct.TIM_ICFilter = 10;// 滤波器
TIM_ICInit(TIM4,&TIM_ICInitStruct);
//清除定时器溢出中断标志位
TIM_ClearFlag(TIM4,TIM_FLAG_Update);
//配置溢出中断
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
//设置计数器初始值为0
TIM_SetCounter(TIM4,0);
//开启定时器
TIM_Cmd(TIM4,ENABLE);
}
3.编码器读取速度
/*******************
编码器速度读取函数
*入口参数：定时器2/4
*******************/
int Read_Speed(int TIMx)
{
int value_1;
switch(TIMx)
{
//int ? short
case 2:value_1=(short)TIM_GetCounter(TIM2);TIM_SetCounter(TIM2,0);break; //先读取编码器的计数值，然后清零计数值，目的是直接获得速度，而不用计数值相减来算速度
case 4:value_1=(short)TIM_GetCounter(TIM4);TIM_SetCounter(TIM4,0);break;
default :value_1 = 0;
}
//这里TIM_GetCounter()函数的返回值是uint16_t ,而我们的返回值是有符号的int类型，所以这里强制类型转化一下
return value_1;
}
4.电机驱动函数
//电机初始化函数
void Motor_Init()
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;//复用推挽
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);
}
//限幅函数
void Limit(int *motorA ,int *motorB)
{
if(*motorA > PWM_MAX)*motorA=PWM_MAX;
if(*motorA < PWM_MIN)*motorA=PWM_MIN;
if(*motorB > PWM_MAX)*motorB=PWM_MAX;
if(*motorB < PWM_MIN)*motorB=PWM_MIN;
}
//绝对值函数
int abs(int p)
{
int q;
q = p>0?p:(-p);
return q;
}
//电机驱动函数
//入口参数：PID运算完成后最终PWM值
void Load(int moto1,int moto2)
{
//判断正反转
if(moto1>0)Ain1=1,Ain2=0;
else Ain1=0,Ain2=1;
//输入PWM的绝对值
TIM_SetCompare1(TIM1,abs(moto1));
if(moto2>0)Bin1=1,Bin2=0;
else Bin1=0,Bin2=1;
TIM_SetCompare4(TIM1,abs(moto2));
}
//电机为10KHZ
//72000000/7200 = 10 000 HZ
#define PWM_MAX 7200
#define PWM_MIN -7200
extern int MOTO1,MOTO2;
#define Ain1 PBout(14)
#define Ain2 PBout(15)
#define Bin1 PBout(13)
#define Bin2 PBout(12)5.外部中断
dmp读取是用的中断读取，10ms读取一次，所以我们要配置MPU6050的外部中断
void MPU6050_EXTI_Init(void)
{
//开启时钟
//因为是复用功能，所以开启AFIO
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);
//配置GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IPU;//GPIO_Mode_AF_PP;复用推挽
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_5;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
//映射GPIO和外部中断
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);
//配置EXTI结构体
EXTI_InitTypeDef EXTI_InitStruct;
EXTI_InitStruct.EXTI_Line = EXTI_Line5;//中断线
EXTI_InitStruct.EXTI_LineCmd = ENABLE;//使能中断线
EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;//中断模式
EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿触发
EXTI_Init(&EXTI_InitStruct);
}
void NVIC_Config()
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
//外部中断
NVIC_InitTypeDef NVIC_InitStruct;
NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;
NVIC_Init(&NVIC_InitStruct);
//串口
NVIC_InitStruct.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority =0;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1;
NVIC_Init(&NVIC_InitStruct);
}
void EXTI9_5_IRQHandler(void)
写中断服务函数，里面做的是PID算法
void EXTI9_5_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line5) == SET)//MPU6050外部中断是PB5
{
if(PBin(5) == 0)//引脚定义为外部上拉，而外部中断触发模式是下降沿，这里就是再进行下降沿的检测
{
EXTI_ClearITPendingBit(EXTI_Line5);
//1.采集编码器数据和MPU6050的角度信息
Encoder_Left = -Read_Speed(2);//因为电机是相对安装的，即左右电机相差180度，为了让编码器输出极性一致，
Encoder_Right = Read_Speed(4);//所以需要取反，或者调转一下极性也可以
mpu_dmp_get_data(&Pitch,&Roll,&Yaw);
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
//2.将数据压入闭环控制中，计算输出控制量
Velocity_out = Velocity(Target_Speed,Encoder_Left,Encoder_Right);
Vertical_out = Vertical(Velocity_out+Med_Angle,Pitch,gyroy);
Turn_out = Turn(gyroz);
PWM_out = Vertical_out;
//3.把控制量加载到电机上
MOTO1 = PWM_out-Turn_out;//这里一加一减是因为两个电机是反向安装的，实际上就是同向加减了
MOTO2 = PWM_out+Turn_out;
Limit(&MOTO1,&MOTO2);
Load(MOTO1,MOTO2);
}
}
}
四、小车
本人不会打板，所以选择了做一回杜邦线战神！
调了直立环和速度环，但是不理想，不能很好的回位
调试
串级的PID调试时应该断掉两个环的连接
机械中值：把平衡小车放在地面上，从前向后以及从后向前绕电机轴旋转平衡小车，两次的向另一边倒下的角度的中值，就是机械中值。
直立环
Kp极性：
极性错误：小车往哪边倒，车轮就往反方向开，会使得小车加速倒下。
极性正确：小车往哪边倒，车轮就往哪边开，以保证小车有直立的趋势。
Kp大小：
Kp一直增加，直到出现大幅低频震荡。
Kd极性：
极性错误：拿起小车绕电机轴旋转，车轮反向转动，无跟随。
极性正确：拿起小车绕电机轴旋转，车轮同向转动，有跟随。
Kd大小：
Kd一直增加，直到出现高频震荡。
直立环调试完毕后，对所有确立的参数乘以0.6作为最终参数。
原因：之前得到的参数都是Kp、Kd最大值，根据工程经验平衡小车的理想参数为最大参数乘以0.6求得。
结果：乘以0.6后，小车的抖动消失，但同时直立效果也变差。待下面加入速度环就能得到更好的性能。
在调试速度环参数极性时：需要去掉（注释掉）直立环运算
在调试速度环参数大小时：再次引入（取消注释）直立环运算
Kp&Ki：
线性关系、Ki=(1/200)*Kp、仅调Kp即可。
Kp&Ki极性：
极性错误：手动转动其中一个车轮，另一车轮会以同样速度反向旋转——典型负反馈。
极性正确：手动转动其中一个车轮，两个车伦会同向加速，直至电机最大速度——典型正反馈。
Kp&Ki大小：
增加Kp&Ki，直至：小车保持平衡的同时，速度接近于零。且回位效果较好。
转向环
Kp极性：
极性错误：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势一致——典型正反馈。
极性正确：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势相反——典型负反馈。
Kp大小：
加大Kp，直至走直线效果较好，且无剧烈抖动。
参考于b站up主天下行走的平衡车教学
