【STM32F411 Nucleo试用体验】5、万事俱备，只差平衡（结项帖）

ti

距离上一个帖子，也有半年了，刚刚挨了福利妹一顿批，赶紧跑来把帖子补上。。

在上一个帖子里，已经成功过获取了车体倾角和车体倾角角速度。这是两个在平衡控制过程中极为重要的两个反馈。

简要的说一下PID，在这个控制过程里，只用到了PD，嗯，就PD就够了，千万别加I
P的作用是修正车体倾角误差->思维导向：如何将一个将要倾倒的车扶正（别说用手，当然是用它的轮子）？很简单，向着它倾倒的方向输出一个足够大小的加速度即可，加速度越大，修正的越快速。所以P*倾角误差出来的是车体需求的加速度。
D的作用是角速度阻尼->思维导向：如何阻碍角速度的变化？还是很简单，让车向着角速度方向输出一个合适大小的加速度抵消水平轴上的重力分量即可。不要问我为什么要加阻尼，就是，没有阻尼就会震荡。

那好，来看看程序

AVR_Speed=(Stell_Speed_1+Stell_Speed_2)/2;
SP_con=(AVR_Speed - AimSP) * Pspeed + (Stell_Drift_1+Stell_Drift_2) / 2 * Pdrift;
if(SP_con>10.0f) SP_con=10.0f;
if(SP_con<-10.0f) SP_con=-10.0f;
Acc_Need=CAR_Gyro_X * PGx + (CAR_Angle - BanAng - SP_con) * PAx;
if(Acc_Need>4.0f) Acc_Need=4.0f;
if(Acc_Need<-4.0f) Acc_Need=-4.0f;

复制代码

AVR_Speed ：左车轮和又车轮的平均速度，即车体当前的车速
CAR_Angle ：车体角度（含安装误差）
BanAng：车体倾角修正值
SP_con：目标车体倾角修正值（有点绕口，一会儿解释），但是，BanAng + SP_con比较好理解了，他俩的和是目标车体倾角。 (CAR_Angle - BanAng - SP_con) 就是车体倾角误差了，它和PAx（比例系数）的乘积就是为修正车体倾角所需的加速度。
CAR_Gyro_X：车体倾角角速度，CAR_Gyro_X * PGx（微分系数）就是角速度阻尼了。
Acc_Need：最终输出加速度

当然还有关键一步就是对PD两个参数的参数整定了。

可以看到上面的程序中还有一行代码也用到了PID,
SP_con=(AVR_Speed - AimSP) * Pspeed + (Stell_Drift_1+Stell_Drift_2) / 2 * Pdrift;
是一个PI控制，这句是用于车速控制的，，
AimSP：目标车速，(AVR_Speed - AimSP) * Pspeed 对车速误差做比例控制
Stell_Drift_1、Stell_Drift_2：左右车轮累计误差位移量，那么 (Stell_Drift_1+Stell_Drift_2) / 2 * Pdrift就是对车速误差做积分控制。
好啦，有了这两个控制，车力得住也不会乱跑了。

主要的控制代码：

static void MotorContorl(float AimSpeed1,float AimSpeed2,float NowSpeed1,float NowSpeed2)
{
static const float P_Speed=70.0f,P_Acc=5.0f;
static float Last_Speed1_Eror,Last_Speed2_Eror,Speed1_Eror,Speed2_Eror,Speed1_ACC,Speed2_ACC;
static float PWM1,PWM2;
u8 FX1,FX2;
static u16 PWMout1,PWMout2;
Speed1_Eror=AimSpeed1-NowSpeed1;
Speed2_Eror=AimSpeed2-NowSpeed2;
Speed1_ACC=(Speed1_Eror-Last_Speed1_Eror)/Control_Period;
Speed2_ACC=(Speed2_Eror-Last_Speed2_Eror)/Control_Period;
Last_Speed1_Eror=Speed1_Eror;
Last_Speed2_Eror=Speed2_Eror;
PWM1+= Speed1_Eror * P_Speed + Speed1_ACC * P_Acc;
PWM2+= Speed2_Eror * P_Speed + Speed2_ACC * P_Acc;
if(PWM1>PWM_Config_Limit) PWM1=PWM_Config_Limit;
if(PWM1<-PWM_Config_Limit) PWM1=-PWM_Config_Limit;
if(PWM2>PWM_Config_Limit) PWM2=PWM_Config_Limit;
if(PWM2<-PWM_Config_Limit) PWM2=-PWM_Config_Limit;
if(PWM1>0) FX1=1;
else
{
FX1=0;
}
if(PWM2>0) FX2=1;
else
{
FX2=0;
}
PWMout1=fabsf(PWM1)+Motor_DEAD_PWM;
PWMout2=fabsf(PWM2)+Motor_DEAD_PWM;
Call_PWM(FX1,FX2,PWMout1,PWMout2);
}
void BanlanceControl(void)
{
static float AVR_Speed;
static float PAx=-0.20f,PGx=0.05f,Pspeed=10.0f,Pdrift=1.0f,P_Turn=-0.005f;
float Acc_Need,SP_con;
if(Stell_Drift_1 > 10) Stell_Drift_1=10;
if(Stell_Drift_1 < -10) Stell_Drift_1=-10;
if(Stell_Drift_2 > 10) Stell_Drift_2=10;
if(Stell_Drift_2 < -10) Stell_Drift_2=-10;
AVR_Speed=(Stell_Speed_1+Stell_Speed_2)/2;
SP_con=(AVR_Speed -AimSP) * Pspeed + (Stell_Drift_1+Stell_Drift_2) / 2 * Pdrift;
if(SP_con>10.0f) SP_con=10.0f;
if(SP_con<-10.0f) SP_con=-10.0f;
Acc_Need=CAR_Gyro_X * PGx + (CAR_Angle - BanAng - SP_con) * PAx;
if(Acc_Need>4.0f) Acc_Need=4.0f;
if(Acc_Need<-4.0f) Acc_Need=-4.0f;
Speed1= Stell_Speed_1 + Acc_Need + CAR_Gyro_Z * P_Turn;
// SP_con=(Stell_Speed_2 - Speed2_in) * Pspeed;
// if(SP_con>4.0f) SP_con=4.0f;
// if(SP_con<-4.0f) SP_con=-4.0f;
Speed2= Stell_Speed_2 + Acc_Need - CAR_Gyro_Z * P_Turn;
// Speed1=Speed2=Q_ANGLE.Pitch;
if(Speed1>8.0f) Speed1=8.0f;
else if(Speed1<-8.0f) Speed1=-8.0f;
if(Speed2>8.0f) Speed2=8.0f;
else if(Speed2<-8.0f) Speed2=-8.0f;
MotorContorl(Speed1,Speed2,Stell_Speed_1,Stell_Speed_2);
}