一个简单的电机与驱动PID算法

haidaowang

一个简单的电机与驱动PID算法
) z+ M( Z: }+ _( G
) `6 p9 P+ F2 c. b; y9 zPID
8 `; F6 l  Z! ?- \6 o
8 E/ i8 }  w% E

• #include <hidef.h>      /* common defines and macros */
• #include "derivative.h"      /* derivative-specific definitions */
• #include <mc9s12xs128.h>
• //定义PID参数
• #define VV_KPVALUE 3       //比例
• #define VV_KIVALUE 40     //积分
• #define VV_KDVALUE 3     //微分
• #define VV_MAX 10000       //返回的最大值,是pwm的周期值
• #define VV_MIN 0
• #define VV_DEADLINE 0X08   //速度PID，设置死区范围
• typedef struct PID       //定义数法核心数据
• {
• signed int vi_Ref;      //速度PID，速度设定值
• signed int vi_FeedBack;  //速度PID，速度反馈值
• signed long vi_PreError;  //速度PID，前一次，速度误差,,vi_Ref - vi_FeedBack
• signed long vi_PreDerror; //速度PID，前一次，速度误差之差，d_error-PreDerror;
• unsigned int v_Kp;      //速度PID，Ka = Kp
• unsigned int v_Ki;      //速度PID，Kb = Kp * ( T / Ti )
• unsigned int v_Kd;      //速度PID，
• signed long vl_PreU;    //电机控制输出值
• }PID;
• PID  sPID;                //  PID Control Structure
• void PIDInit(void)
• {
• sPID.vi_Ref = 0 ;        //速度设定值
• sPID.vi_FeedBack = 0 ;  //速度反馈值
• sPID.vi_PreError = 0 ;   //前一次，速度误差,,vi_Ref - vi_FeedBack
• sPID.vi_PreDerror = 0 ;   //前一次，速度误差之差，d_error-PreDerror;
• sPID.v_Kp = VV_KPVALUE;
• sPID.v_Ki = VV_KIVALUE;
• sPID.v_Kd = VV_KDVALUE;
• sPID.vl_PreU = 0 ;      //电机控制输出值
• }
• unsigned int v_PIDCalc( PID *pp )
• {
• signed long  error,d_error,dd_error;
•   error = (signed long)(pp->vi_Ref - pp->vi_FeedBack); // 偏差计算
•   d_error = error - pp->vi_PreError;
•   dd_error = d_error - pp->vi_PreDerror;
•   pp->vi_PreError = error; //存储当前偏差
• pp->vi_PreDerror = d_error;
• if( ( error < VV_DEADLINE ) && ( error > -VV_DEADLINE ) ); //设置调节死区
•               //速度PID计算
•   pp->vl_PreU += (signed long)(  pp -> v_Kp * d_error + pp -> v_Ki * error  + pp->v_Kd*dd_error);
• else if( pp->vl_PreU >= VV_MAX )   //速度PID，防止调节最高溢出
•   pp->vl_PreU = VV_MAX;
• else if( pp->vl_PreU <= VV_MIN ) //速度PID，防止调节最低溢出
•   pp->vl_PreU = VV_MIN;
• else
• ;
•    return ( pp->vl_PreU  );  // 返回预调节占空比
• }
• void main(void) {
•   /* put your own code here */
•    Initmcu();
•     IncPIDInit();
• int g_CurrentVelocity=0;   //全局变量也初始化
• int g_Flag=0;                //全局变量也初始化
• EnableInterrupts;
•   While(1)
• {
•    if (g_Flag&&vi_FeedBack)
•       {
•           PWMOUT+=  v_PIDCalc( PID *pp );
•             g_Flag&=~ vi_FeedBack;
• }
• }
• }
•   for(;;) {
•     _FEED_COP(); /* feeds the dog */
•   } /* loop forever */
•   /* please make sure that you never leave main */
• }
  

5 O( n/ E) _% ?6 i) f6 s

xixihahaheihei

这个好，不知道能不能调通