PID源码讲解

有人让我解释一下我程序里面PID计算的过程,这里就插一节吧,就是介绍下我的PID,当然,还很不完善,后期肯定还要经过很多改动才行,这里就先介绍下现在的"初级版"吧.
void PID_CAL(void)                PID计算函数
{
        static float thr=0,rool=0,pitch=0,yaw=0;          控制量
        static float rool_i=0,pitch_i=0;                         积分
        
        int16_t Motor1,Motor2,Motor3,Motor4;             四个电机输出
        
        IMU_TEST();                                                   计算姿态
        GET_EXPRAD();                                             获得控制量
////////////////////////////////////////////////////////////////////////////////
        rool         = PID_RP.P * DIF_ANGLE.X;                    roll方向的P计算
        
        if(Q_ANGLE.Rool>-0.1 && Q_ANGLE.Rool<0.1)   判断I是否需要清零
        {                                                                            
                rool_i = 0;
        }
        rool_i -= PID_RP.I * Q_ANGLE.Rool;                   I计算
        PID_RP.IMAX = DIF_ANGLE.X * 10;                    积分限幅
        if(PID_RP.IMAX<0)        
        {
                PID_RP.IMAX = (-PID_RP.IMAX) + 100;
        }
        else
        {
                PID_RP.IMAX += 100;
        }
        if(rool_i>PID_RP.IMAX)         rool_i = PID_RP.IMAX;
        if(rool_i<-PID_RP.IMAX)        rool_i = -PID_RP.IMAX;
        rool += rool_i;                                                            积分
        
        rool -= PID_RP.D * GYRO_F.X;                                      D计算
///////////        
        pitch         = PID_RP.P * DIF_ANGLE.Y;                                 同上
                
        if(Q_ANGLE.Pitch>-0.1 && Q_ANGLE.Pitch<0.1)
        {
                pitch_i = 0;
        }
        pitch_i -= PID_RP.I * Q_ANGLE.Pitch;
        if(PID_RP.IMAX<0)        
        {
                PID_RP.IMAX = (-PID_RP.IMAX) + 100;
        }
        else
        {
                PID_RP.IMAX += 100;
        }
        if(PID_RP.IMAX<0)        PID_RP.IMAX = -PID_RP.IMAX;
        if(pitch_i>PID_RP.IMAX)         pitch_i = PID_RP.IMAX;
        if(pitch_i<-PID_RP.IMAX)        pitch_i = -PID_RP.IMAX;
        pitch += pitch_i;
        
        pitch -= PID_RP.D * GYRO_F.Y;
/////////////
        GYRO_I[0].Z += EXP_ANGLE.Z/3000;                              yaw方向就简单的用了陀螺的积分    PD
        yaw = -10 * GYRO_I[0].Z;
        
        yaw -= 3 * GYRO_F.Z;
//        
        thr = RC_DATA.THROTTLE+400;
////////////////////////////////////////////////////////////////////////////////将控制量输出给电机
        Motor1=(int16_t)(thr + rool - pitch + yaw);
        Motor2=(int16_t)(thr + rool + pitch - yaw);
        Motor3=(int16_t)(thr - rool + pitch + yaw);
        Motor4=(int16_t)(thr - rool - pitch - yaw);
        if(FLY_ENABLE && (RC_DATA.THROTTLE>-400))                          和解锁有关,未解锁或油门太低电机禁止转动
                MOTO_PWMRFLASH(Motor1,Motor2,Motor3,Motor4);
        else
                MOTO_PWMRFLASH(0,0,0,0);
}

程序就这么简单,当然还需要很多改进,PID参数也需要慢慢调试才行

转载http://www.anotc.com/Articles/Browse/1