PID控制学习

 

注释：

erro：误差，当前值与目标值的差值

integral：积分，误差值的积分，用于补充误差

derivative：微分，误差值的微分，用于修正震荡

output： 输出，三个比例值乘以三个参数，修改的是三个比例值：KP KI KD

对于循迹小车：

erro：偏差值，使用灰度传感器，八个灰度传感器，每个传感器输出0 1数字量，八个传感器间的距离为固定值，我们对于偏差值的定义如下：

4 5全亮偏差值为0

4 为-1 ， 4 3为-2 ， 3为-3 ， 2 3为-4 ， 2为-5 ......

5为1 ， 5 6为2 ......

integral：宏观上看，是小车对于黑线的预判程度

derivative：宏观上看，是小车的摆动程度

output：是小车pwm的调整值，改变轮子的转速

还有一种办法为加权算偏差值，笔者还未采用，贴一张图以后使用

 具体PID代码：

float previous_error=0;
float integral=0;

PID DisPdate=   //位置PID结构体
{
    5,        //赋值比例值    kp
    30,        //赋值积分值    ki
    3         //赋值微分值    kd
};
int PositionPID(float previous_error,float integral,PID pid)
{
    float KP=pid.kp,KI=pid.ki,KD=pid.kd;
    float error,derivative;
    volatile float pwm;
    
    if(gary_sensor_read(L4_IO,L4_PIN)==0&&gary_sensor_read(L5_IO,L5_PIN)==0)  //gary_sensor_read检测灰度传感器的数值，1为亮 0为灭，0为检测到黑线
        error=0;
    else if(gary_sensor_read(L4_IO,L4_PIN)==0)
        error=-1;
    else if(gary_sensor_read(L4_IO,L4_PIN)==0&&gary_sensor_read(L3_IO,L3_PIN)==0)
        error=-2;
    else if(gary_sensor_read(L3_IO,L3_PIN)==0)
        error=-3;
    else if(gary_sensor_read(L3_IO,L3_PIN)==0&&gary_sensor_read(L2_IO,L2_PIN)==0)
        error=-4;
    else if(gary_sensor_read(L2_IO,L2_PIN)==0)
        error=-5;
    else if(gary_sensor_read(L1_IO,L1_PIN)==0&&gary_sensor_read(L2_IO,L2_PIN)==0)
        error=-6;
    else if(gary_sensor_read(L1_IO,L1_PIN)==0)
        error=-7;
    
    
    else if(gary_sensor_read(L5_IO,L5_PIN)==0)
        error=1;
    else if(gary_sensor_read(L5_IO,L5_PIN)==0&&gary_sensor_read(L6_IO,L6_PIN)==0)
        error=2;
    else if(gary_sensor_read(L6_IO,L6_PIN)==0)
        error=3;
    else if(gary_sensor_read(L6_IO,L6_PIN)==0&&gary_sensor_read(L7_IO,L7_PIN)==0)
        error=4;
    else if(gary_sensor_read(L7_IO,L7_PIN)==0)
        error=5;
    else if(gary_sensor_read(L7_IO,L7_PIN)==0&&gary_sensor_read(L8_IO,L8_PIN)==0)
        error=6;
    else if(gary_sensor_read(L8_IO,L8_PIN)==0)
        error=7;
    
    integral += error;
    derivative = (error - previous_error);
    
    pwm = KP*error + KI*integral + KD*derivative;
    previous_error = error;
    
    return pwm;    //这里返回的是整数，若想要更精确可换为小数
}

main函数中：

...初始化省略
while(1)
{
    adjustment=PositionPID(previous_error,integral,DisPdate);   //PID输出调整值
    res1 = faultpwm+adjustment;    //调整值加上默认值
    res2 = faultpwm-adjustment;
    
        
    if(res1>600)
        res1 = 600;
    else if(res1<0)
        res1 = 0;
            
    if(res2>600)
        res2 = 600;
    else if(res2 <0)
        res2 = 0;       //限幅，若无此步，小车会失速
            
            
    if(adjustment>0)
    {
        motorL_set(1,res1);
        motorR_set(1,res2);    //调整左右电机速度
    }
    else if(adjustment<0)
    {
        motorR_set(1,res2);
        motorL_set(1,res1);
    }
    else if(gary_sensor_read(L1_IO,L1_PIN)==1&&gary_sensor_read(L2_IO,L2_PIN)==1&&gary_sensor_read(L3_IO,L3_PIN)==1&&gary_sensor_read(L4_IO,L4_PIN)==1&&gary_sensor_read(L5_IO,L5_PIN)==1&&gary_sensor_read(L6_IO,L6_PIN)==1&&gary_sensor_read(L7_IO,L7_PIN)==1&&gary_sensor_read(L8_IO,L8_PIN)==1)
    
        car_stop();        //若灯全亮则无黑场，此时小车停止
}