智能车舵机PD运算方法.doc

《智能车舵机PD运算方法.doc》由会员上传分享，免费在线阅读，更多相关内容在工程资料-天天文库。

1、/**********************舵机增量式PID算法***********************    double ref = 0;//设置参数设定值double feb = 0;//采样反馈过程值int pwm_var = 0;    //PID调整量int PWM_out = 0; //PWM输出量double Uo = 0;double Ek = 0;double Ei = 0;double Ed = 0; #define Kp   8        //PID调节的比例常数#define Ti   0.05     //PID调节的积分常数#define

2、 Td   0.02     //PID调节的微分时间常数#define T    0.02     //采样周期#define Kpp   Kp * ( 1 + (T / Ti) + (Td / T) )#define Ki   (-Kp) * ( 1 + (2 * Td / T ) )#define Kd    Kp * Td / T//#define Kpp 4//#define Ki  0.8//#define Kd  20//误差的阀值，小于这个数值的时候，不做PID调整，避免误差较小时频繁调节引起震荡#define Emin 3//调整值限幅，防止积分饱和#defin

3、e Umax 100#define Umin -100//输出值限幅#define Pmax 15500#define Pmin 200/////////////////////////////////////////////////////////////////////////    PID运算    ///////8void pid_ctrl(void){  Ek = ref - feb;       //差值运算    if( fabs(Ek) < Emin ) //误差的阀值(死区控制??)  {    pwm_var = 0;  }  else   {    Uo =

4、 Kpp*Ek + Ki*Ei + Kd*Ed;//PID计算    Ed = Ei;    Ei = Ek;        pwm_var = (int)Uo;    //制转化调整量,PWM为整数        if(pwm_var >= Umax)pwm_var = Umax;      //调整值限幅，防止积分饱和    if(pwm_var <= Umin)pwm_var = Umin;    //调整值限幅，防止积分饱和  }    PWM_out += pwm_var;        //调整PWM输出    if(PWM_out > Pmax)PWM_out = 

5、Pmax;    //输出值限幅  if(PWM_out < Pmin)PWM_out = Pmin;    //输出值限幅    TBCCR1 = PWM_out;//输出给寄存器,改变PWM占空比}8#defineG_Kp8//P8#defineG_Ki15//I#defineG_Kd0//Dinterror;intpre_error;intlast_error;intU_error;uintU_Pre1=9000;//摆头舵机中心位置uintU_Pre2=8600;//方向舵机中心位置ucharsum;uchartemp,tt,ss;/*******************

6、*******************************函数：delay_ms()描述：延迟x*10微秒**************************************************/voiddelay(uinttime){while(time--){_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);}}voiddisplay(){sum=0;error=0;/**********************

7、***********/PORTA=0x00;delay(100);if(PORTA_PA0==1)8{sum++;error-=12;}/*********************************/PORTA=0x02;delay(100);if(PORTA_PA0==1){sum++;error-=10;}/*********************************/PORTA=0x04;delay(100);if(PORTA_PA0==1){sum++;er