注册 登录  
 加关注
   显示下一条  |  关闭
温馨提示!由于新浪微博认证机制调整,您的新浪微博帐号绑定已过期,请重新绑定!立即重新绑定新浪微博》  |  关闭

FY

Johnson 's Blog

 
 
 

日志

 
 

PID源码讲解  

2016-04-28 11:00:21|  分类: 飞控 |  标签: |举报 |字号 订阅

  下载LOFTER 我的照片书  |
http://www.anotc.com/Articles/Browse/1

有人让我解释一下我程序里面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参数也需要慢慢调试才行

  评论这张
 
阅读(10)| 评论(0)
推荐 转载

历史上的今天

在LOFTER的更多文章

评论

<#--最新日志,群博日志--> <#--推荐日志--> <#--引用记录--> <#--博主推荐--> <#--随机阅读--> <#--首页推荐--> <#--历史上的今天--> <#--被推荐日志--> <#--上一篇,下一篇--> <#-- 热度 --> <#-- 网易新闻广告 --> <#--右边模块结构--> <#--评论模块结构--> <#--引用模块结构--> <#--博主发起的投票-->
 
 
 
 
 
 
 
 
 
 
 
 
 
 

页脚

网易公司版权所有 ©1997-2017