freescale智能车舵机pid运算

《freescale智能车舵机pid运算》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

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调节的比例常数#

2、define Ti   0.05     //PID调节的积分常数#define 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 K

3、d  20//误差的阀值，小于这个数值的时候，不做PID调整，避免误差较小时频繁调节引起震荡#define Emin 3//调整值限幅，防止积分饱和#define Umax 100#define Umin -100//输出值限幅#define Pmax 15500#define Pmin 200/////////////////////////////////////////////////////////////////////////    PID运算    ///////void pid_ctr

4、l(void){  Ek = ref - feb;       //差值运算    if( fabs(Ek) < Emin ) //误差的阀值(死区控制10??)  {    pwm_var = 0;  }  else   {    Uo = Kpp*Ek + Ki*Ei + Kd*Ed;//PID计算    Ed = Ei;    Ei = Ek;        pwm_var = (int)Uo;    //制转化调整量,PWM为整数        if(pwm_var >= Umax)pwm_v

5、ar = Umax;      //调整值限幅，防止积分饱和    if(pwm_var <= Umin)pwm_var = Umin;    //调整值限幅，防止积分饱和  }    PWM_out += pwm_var;        //调整PWM输出    if(PWM_out > Pmax)PWM_out = Pmax;    //输出值限幅  if(PWM_out < Pmin)PWM_out = Pmin;    //输出值限幅    TBCCR1 = PWM_out;//输出给寄存器,

6、改变PWM占空比}#defineG_Kp8//P8#defineG_Ki15//I#defineG_Kd0//Dinterror;intpre_error;intlast_error;10intU_error;uintU_Pre1=9000;//摆头舵机中心位置uintU_Pre2=8600;//方向舵机中心位置ucharsum;uchartemp,tt,ss;/**************************************************函数：delay_ms()描述：延迟x*

7、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);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm

8、(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_as