代码收藏家 串级PID 位置环+速度环
1位置环和速度环的串级pid，首先要记住，位置环的输出是速度环的输入，最后控制输出为速度环的输出。
速度环的PID控制器 代码如下
float Velocity_KP_A=400,Velocity_KI_A=300;
int Incremental_PI_A (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //计算偏差
Pwm+=Velocity_KP_A*(Bias-Last_bias)+Velocity_KI_A*Bias;
if(Pwm>7200)Pwm=7200;
if(Pwm<-7200)Pwm=-7200;
Last_bias=Bias; //Save the last deviation //保存上一次偏差
return Pwm;
}速度环只用Kp和Ki控制即可
位置环PID控制器
int Location_pid(float Encoder,float Target)
{
static float Bias,Speed,Last_bias,Integral_bias;
Bias=Target-Encoder; //Calculate the deviation //计算偏差
Integral_bias+=Bias;//求出偏差的积分
Speed+=Location_KP*Bias+Location_KI*Integral_bias+Location_KD*(Bias-Last_bias);
Last_bias=Bias;//保存上次偏差
return Speed;
}位置环用Kp和Kd即可（只用Kp好像也行的）
2如何将位置环和速度环串在一起
对于位置环
目标为行走的距离，需要转向的度数 （可以把距离与度数转化为脉冲数，方便进行PID控制）
反馈为编码器捕获脉冲数的累计数
每次要用位置环时必须先将脉冲累计数清零
距离转化为脉冲的方法
Target_Encoder_sum_A=distence/(PI*WD)*(EncoderMultiples*MD*Hall_13);
//The encoder octave depends on the encoder initialization Settings
//编码器倍频数，取决于编码器初始化设置
#define EncoderMultiples 4
//Motor_gear_ratio
//电机减速比
#define MD 20
//Number_of_encoder_lines
//编码器精度
#define Hall_13 13
//4WD wheel tire diameter series
//四驱车轮胎直径
#define WD 0.045f
//Encoder data reading frequency
//编码器数据读取频率
#define CONTROL_FREQUENCY 100
//half wheelspacing //半轮距
#define Half_wheelspacing 0.0644f
//half axlespacing //半轴距
#define Half_axlespacing 0.010f
#define PI 3.1415f //PI //圆周率
//这里加将位置环的输入直接编程距离（m）的函数所有普通直行都用这个
void Set_Distence_m(uint16_t distence)
{
PID_flag=1;
Turn_Finish_flag=7;
Encoder_A_sum=0;
Encoder_B_sum=0;
Encoder_C_sum=0;
Encoder_D_sum=0;
Target_Encoder_sum_A=distence/(PI*WD)*(EncoderMultiples*MD*Hall_13);
Target_Encoder_sum_B=Target_Encoder_sum_A;
Target_Encoder_sum_C=Target_Encoder_sum_A;
Target_Encoder_sum_D=Target_Encoder_sum_A;
}旋转度数转化为脉冲的方法
//转角有左转90，右转90，和转180三种情况。
//左转为90， 右转为-90，转180
void Set_Turn(int i)
{
PID_flag=1;
float Car_Turn_val;
float spin90_val;
if(i==180)//原地左转180
{
spin90_val=0.25*PI*Half_wheelspacing*2;
Turn_Finish_flag=1;
Car_Turn_val=spin90_val/(PI*WD)*(EncoderMultiples*MD*Hall_13)*2;//计算出需要的累计脉冲
Target_Encoder_sum_A=-Car_Turn_val;
Target_Encoder_sum_B=-Car_Turn_val;
Target_Encoder_sum_C=Car_Turn_val;
Target_Encoder_sum_D=Car_Turn_val;
}
else if(i==90)//左转 左轮为负，右轮为正，
{
spin90_val=0.25*PI*Half_wheelspacing*2;
Turn_Finish_flag=2;
Car_Turn_val=spin90_val/(PI*WD)*(EncoderMultiples*MD*Hall_13);//计算出需要的累计脉冲
Target_Encoder_sum_A=-Car_Turn_val;
Target_Encoder_sum_B=-Car_Turn_val;
Target_Encoder_sum_C=Car_Turn_val;
Target_Encoder_sum_D=Car_Turn_val;
}
Encoder_A_sum=0;
Encoder_B_sum=0;
Encoder_C_sum=0;
Encoder_D_sum=0;
}3运用pid将所需的脉冲计算出来，然后把单位同意换算为速度 m/s 方便传入速度环，这里会以最大限幅去执行
/**************************************************************************
位置环输出速度
**************************************************************************/
void Drive_Motor()
{
static float amplitude=1,Turn_amplitude=0.5; //Wheel target speed limit //车轮目标速度限幅
float A_Target,B_Target,C_Target,D_Target;
//这里是脉冲算的 要传入速度环还需将位置环计算出来的脉冲转换为速度
A_Target=Location_pid(-Encoder_A_sum,Target_Encoder_sum_A);
B_Target=Location_pid(-Encoder_B_sum,Target_Encoder_sum_B);
C_Target=Location_pid(-Encoder_C_sum,Target_Encoder_sum_C);
D_Target=Location_pid(-Encoder_D_sum,Target_Encoder_sum_D);
MOTOR_A.Target=A_Target*CONTROL_FREQUENCY*(WD*PI)/(EncoderMultiples*MD*Hall_13);
MOTOR_B.Target=B_Target*CONTROL_FREQUENCY*(WD*PI)/(EncoderMultiples*MD*Hall_13);
MOTOR_C.Target=C_Target*CONTROL_FREQUENCY*(WD*PI)/(EncoderMultiples*MD*Hall_13);
MOTOR_D.Target=D_Target*CONTROL_FREQUENCY*(WD*PI)/(EncoderMultiples*MD*Hall_13);
if(Turn_Finish_flag==2)//左90
{
MOTOR_A.Target=-float_abs(MOTOR_A.Target);
MOTOR_B.Target=-float_abs(MOTOR_B.Target);
MOTOR_C.Target=float_abs(MOTOR_C.Target);
MOTOR_D.Target=float_abs(MOTOR_D.Target);
}
else if(Turn_Finish_flag==3)//右90
{
MOTOR_A.Target=float_abs(MOTOR_A.Target);
MOTOR_B.Target=float_abs(MOTOR_B.Target);
MOTOR_C.Target=-float_abs(MOTOR_C.Target);
MOTOR_D.Target=-float_abs(MOTOR_D.Target);
}
if(Turn_Finish_flag==7)
{
//直线速度限幅
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-amplitude,amplitude);
}
else if(Turn_Finish_flag==1||Turn_Finish_flag==2||Turn_Finish_flag==3||Turn_Finish_flag==4||Turn_Finish_flag==5||Turn_Finish_flag==6)
{
//转向速度限幅
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-Turn_amplitude,Turn_amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-Turn_amplitude,Turn_amplitude);
MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-Turn_amplitude,Turn_amplitude);
MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-Turn_amplitude,Turn_amplitude);
}
}4将计算出来呢的素的MOTOR_A.Target传入速度环当作目标速度，最后判断位置环是否执行完成，及判断捕获的脉冲累计数与我们计算的是否相同，相同及完成位置环的控制，如果不用位置环了，就需要清除位置环
void Finish_Turn()
{
static float A,B,C,D,E,F,G,H;
A=float_abs(Encoder_A_sum);
B=float_abs(Encoder_B_sum);
C=float_abs(Encoder_C_sum);
D=float_abs(Encoder_D_sum);
E=float_abs(Target_Encoder_sum_A);
F=float_abs(Target_Encoder_sum_B);
G=float_abs(Target_Encoder_sum_C);
H=float_abs(Target_Encoder_sum_D);
if(Turn_Finish_flag==2||Turn_Finish_flag==3||Turn_Finish_flag==4||Turn_Finish_flag==5||Turn_Finish_flag==6)
{
if(A>(E*0.99)&&B>(F*0.99)&&C>(G*0.99)&&D>(H*0.99))//90度转弯需要的大概脉冲 *0.99作为补偿 理想为1
{
Turn_Finish_flag=0;
}
}
else if(Turn_Finish_flag==1)
{
if(A>(E*1.7)&&B>(F*1.7)&&C>(G*1.7)&&D>(H*1.7))//180度转弯需要的大概脉冲 *1.7作为补偿 理想为2
{
Turn_Finish_flag=0;
}
}
}5位置环的输出作为速度环的输入
void Balance_task()
{
//Get the encoder data, that is, the real time wheel speed,
//and convert to transposition international units
//获取编码器数据，即车轮实时速度，并转换位国际单位
Get_Velocity_Form_Encoder();
if(PID_flag==1)//速度环+位置环控制计算各电机PWM值，PWM代表车轮实际转速
{
Drive_Motor();//位置环输出速度
Finish_Turn();
if(Turn_Finish_flag==1||Turn_Finish_flag==2||Turn_Finish_flag==3||Turn_Finish_flag==4||Turn_Finish_flag==5||Turn_Finish_flag==6||Turn_Finish_flag==7)
{
Location_PID_Finish_flag=1;
MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);
Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm); //MD36电机系列
}
else if(Turn_Finish_flag==0)
{
PID_flag=2;
Location_PID_Finish_flag=0;
Encoder_A_sum=0;
Encoder_B_sum=0;
Encoder_C_sum=0;
Encoder_D_sum=0;
Set_Pwm(0,0,0,0); //MD36电机系列
}
}
else if(PID_flag==0)//速度闭环控制计算各电机PWM值，PWM代表车轮实际转速
{
//单独速度环限幅
Speed_A=target_limit_float(Speed_A,-3.5,3.5);
Speed_B=target_limit_float(Speed_B,-3.5,3.5);
Speed_C=target_limit_float(Speed_C,-3.5,3.5);
Speed_D=target_limit_float(Speed_D,-3.5,3.5);
// MPU6050_pwm=MPU6050_pid(yaw/70,0.2);
// printf("Speed=%lf,yaw=%lf,MPU6050_pwm=%d\r\n",MOTOR_A.Encoder,yaw,MPU6050_pwm);
// MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, Speed_A)-Xunji_Pwm;
// MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, Speed_B)-Xunji_Pwm;
// MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, Speed_C)+Xunji_Pwm;
// MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, Speed_D)+Xunji_Pwm;
MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, Speed_A)-Xunji_Pwm;
MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, Speed_B)-Xunji_Pwm;
MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, Speed_C)+Xunji_Pwm;
MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, Speed_D)+Xunji_Pwm;
Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm); //MD36电机系列
Encoder_A_sum=0;
Encoder_B_sum=0;
Encoder_C_sum=0;
Encoder_D_sum=0;
}
}
