通过平衡车的学习更好的了解和之直观感受PID自动控制过程，感受电子带来的魅力与快乐。
一：理论
直立环：车向那一边倾斜轮子向那边转转动。
目标让小车直立，电机速度为0，即期望角度为0°；也称为机械角度根据MPU6050的安装方法不同会有所误差。
直立控制PD：Motorz_out=Zhili_Kp*(Z_jiao-q-Q_jiao)+Zili_Kd*gyroY
直立环输出=Kp1*（真实角度-期望角度+机械中值）+Kd*角度偏差的微分
速度环+直立环—串级PID：
速度环输入：1.给定速度。2.速度反馈。
输出：角度值（直立环的期望速度输入）
直立环输入：1.给定角度（速度环输出）。2.角度反馈
输出：PWM（直接控制小车）
Motorz_out=Zhili_Kp*(Z_jiao-Q_jiao)+Zili_Kd*gyroY //直立PD控制器
Motots_out =Sudu_Kp*(Z_sudu- Q_sudu)+Sudu_Ki*S(Z_sudu- Q_sudu) //速度PI控制器
直立环输出=Zhili_Kp*（真实角度-期望角度+机械中值）+Zhili_Kd*角度偏差的微分 //角度偏差=真实角度-期望角度
速度环输出=Sudu_Kp*（反馈编码器值-期望编码器值）+Sudu_Ki*编码器偏差的积分
最终输出：Motor_out=Motorz_out+Zhili_Kp*Motots_out
最终输出=Zhili_Kp*真实角度+ Zhili_Kd*角度偏差的微分-Zhili_Kp* [Sudu_Kp*编码器偏差- Sudu_Ki*编码器偏差的积分]
转向环：在保证小车平衡的前提下实现转向
Motory_out=Zhuan_Kp*(Yaw-Q_zuanx)+zhuan_Ki*Zhuan_ji+zhuan_Kd*gyroz;
转向输出=Zhuan_Kp*(真实偏航角-期望偏航角)+zhuan_Ki*偏航积分+zhuan_Kd*偏航加速度；
加载到电机：
LMotor=Motor_out+Motory_out ; RMotor=Motor_out-Motory_out ; Load(LMotor,RMotor);
硬件：
CH-05蓝牙模块：用于控制平衡车转向，前进后退。一定要注意+V5与GNG不要反接，否则将少一杯奶茶！！！！
MPU6050：通过四元素计算出欧拉角获取小车姿态，用于控制车身平衡。
降压模块：用于电机供电 注意！！！千万不要将IN和OuT接反否则会炸板。
整体：后继PCB板经过改善会有一点差别，按照工程焊就完事了。
代码：PID控制器
#include "control.h"
float Q_jiao=0; //机械中值。---在这里修改你的机械中值即可(大11//小0)。
float Target_Speed=0; //期望速度。---二次开发接口，用于控制小车前进后退及其速度。
float Med_AngleZ=0; //期望转向角度
float
Zhili_Kp=-280,//直立环KP、KD（大300，小-280）
Zhili_Ki=-1;//（大1.2，小-1）
float
Sudu_Kp=-0.64,//速度环KP、KI（大0.46，小-0.64）
Sudu_Kd=-0.0032;//(大0.0023，小-0.0032)
float
Zhuan_Kp=-6,//转向环KP(小-5，大4)
Zhuan_Ki=-0.023,//、KI(小-0.015，大0.06)
Zhuan_Kd=-0.4;//Kd（小-0.3，大1.2）
int16_t K0,K1,K2,K3,K4;
#define SPEED_Y 180;
#define SPEED_Z -180;
int Motorz_out,Motors_out,Motory_out;//直立环&速度环&转向环 的输出变量
int Vertical(float Med,float Angle,float gyro_Y);//函数声明
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int ANGl_Z,int gyroZ);
void EXTI9_5_IRQHandler(void)
{
int Motor_out;
if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定
{
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.将数据压入闭环控制中，计算出控制输出量。
/*遥控数据压入*/
if(Serial_GetRxFlag() == 1)
{
K0=USART_RX_BUF[0];//OK
K1=USART_RX_BUF[1]*1.8 ;//OK
K2=USART_RX_BUF[2];//OK
K3=USART_RX_BUF[3];//OK
K4=USART_RX_BUF[4];//OK
}
Med_AngleZ = Med_AngleZ > 180 ? 180:(Med_AngleZ<(-180)?(-180):Med_AngleZ);//限幅
Motorz_out=Vertical(Motorz_out,Pitch-Q_jiao,gyroy); //直立环
Motors_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
Motory_out=Turn(Yaw-Med_AngleZ,gyroz); //转向环
Motor_out=Motorz_out-Zhili_Kp*Motors_out;//最终输出
//3.把控制输出量加载到电机上，完成最终的的控制。
LMotor=Motor_out-Motory_out;//左电机
RMotor=Motor_out+Motory_out;//右电机
Limit(&RMotor,&RMotor); //PWM限幅
Load(LMotor,RMotor); //加载到电机上。
Stop(&Q_jiao,&Pitch);
}
}
}
/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D //Kp1*（【真实角度】）+Kd*角度偏差的微分 （角速度）
入口：期望角度、真实角度、真实角速度
出口：直立环输出
*********************/
int Vertical(float Med,float Angle,float gyroY)//med机械中值，即期望角度angle真实角度，Y加速度
{
int PWM_out;
PWM_out=Zhili_Kp*(Angle)+Zhili_Ki*(gyroY-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);//舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
//2.对速度偏差进行低通滤波
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;
if((K0>10)||(K0<-10)||(K1>10)||(K1<-10)){Q_jiao=K0/40;Encoder_S=0;} //遥控控制时速度积分清除
//4.积分限幅
Encoder_S=Encoder_S>1500?1500:(Encoder_S<(-1500)?(-1500):Encoder_S);
if(stop==1) Encoder_S=0,stop=0;
//5.速度环控制输出计算
PWM_out=Sudu_Kp*EnC_Err_Lowout+Sudu_Kd*Encoder_S;
return PWM_out;
}
/*********************
转向环：系数*Z轴角速度
*********************/
int Turn(int ANGl_Z,int gyroZ)
{
static int Q_zhuan,Zhuab_ji,Zhuan_lvbo,Zhuan_bocan;
float b=0.7;
int PWM_out;
/*Kp针对转向环的约束，Kd针对遥控转向*/
Q_zhuan=ANGl_Z;
Zhuan_lvbo=(1-b)*Q_zhuan+b*Zhuan_bocan;
Zhuan_bocan=Zhuan_lvbo;
Zhuab_ji+=Zhuan_lvbo;
if((K1>10)||(K1<-10)) {Med_AngleZ=K1;}
Zhuab_ji=Zhuab_ji>1500?1520:(Zhuab_ji<(-1500)?(-1500):Zhuab_ji);
PWM_out=(Zhuan_Kp)*Q_zhuan + Zhuab_ji*Zhuan_Ki+Zhuan_Kd*gyroZ;
return PWM_out;
}
机械中值的确定：将小车平稳放置地面，以车轴为对城中心，分别将小车从正副X半轴扶起直到从夫扶起的一边到向另一边，观察俯仰角倾倒的临界值相加除以2取均值。（多试几次以得到的中值更准确）
PID调节方法：
先调直立环：
将直立环和转向环注释，Zhili_Ki=0;调节Zhili_Kp,当车身倾斜角度与车轮转动的方向一至是将数值(Zhili_Kp)取反(-Zhili_Kp),直到出现高频抖动，在调节Zhili_Ki;直到出现低频抖动，说明此时小车接近平衡。
Motorz_out=Vertical(Motorz_out,Pitch-Q_jiao,gyroy); //直立环
// Motors_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
//
// Motory_out=Turn(Yaw-Med_AngleZ,gyroz); //转向环速度环调节：
1：将直立环和转向环注释，Sudu_Kd=0;给Sudu_Kp一个值，瞬间将小车向一个方向倾斜，观察轮子转动方向是否与倾斜方一致。不一致将Sudu_Kp取反。
// Motorz_out=Vertical(Motorz_out,Pitch-Q_jiao,gyroy); //直立环
Motors_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
// Motory_out=Turn(Yaw-Med_AngleZ,gyroz); //转向环2：加入Sudu_Kd环节，转动轮子，轮子是否在短时间能达到最大转速。一般的Kp=200Kd;
3:解开直立环同时时微调
转向环调节：
在直立环与速度环调节平稳的情况下，车身可能会走偏。此时就要调节转向环，同样的先调节Zhuan_Kp，Zhuan_Kd=Zhuan_Ki=0，这里只能凭经验调节一般选取 正负0.5进行调节，逐渐增加或减少直到出现较好的效果。
成功关键：左右电机速度方向是否一致
//1.采集编码器数据&MPU6050角度信息。
Encoder_Left = Read_Speed(2);//电机是相对安装，刚好相差180度，
Encoder_Right = Read_Speed(4);//为了编码器输出极性一致，就需要对其中一个取反。
//OLED显示
OLED_Float(3,2,Encoder_Left,1);//OK
OLED_Float(3,60,Encoder_Right,1);//OK由于编码电机的正负及，AB相项与OI口的连接顺序不同，导致会出现车轮的方向是一致的/但在
程序中转速相等符号相反。此时可以通过OLED显示屏观察转速大小方向。
##遥控器后续整理资料在发布##
##第一次发表文章，各位猿友多多包涵，共同进步##
#电子佬 ：）
源码：链接：https://pan.baidu.com/s/1xn2k5iu4ZbV_-1t3PKBJPA?pwd=95hg
提取码：95hg
4万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
