第五届飞思卡尔智能车电磁组获奖程序

第五届飞思卡尔智能车电磁组获奖程序

#include  <hidef.h>  /*  common  defines  and  macros  */

#include  "derivative.h"  /*  derivative-specific  definitions  */

#include  <stdio.h>

#include  <MC9S12XS128.h>

#include  <stdlib.h>

#include  <math.h>

#pragma  LINK_INFO  DERIVATIVE  "mc9s12xs128"

#define  K  1

#define  N  3 

unsigned  int  i=0,j=0,m=0,n=0;

unsigned  char  str[]={'0'};

signed  int  b=0,c=0,d=0,e=0,f=0,g=0,spe=0,r=0,l,x3,y,z,x1,x2,x4;

int  time;

int  zd=0,flag;

int  pp;

/////////////////////////////////////

////  电机  PID  定义

/////////////////////////////////////

int  SetPoint=0;  //设定目标  Desired  Value 

int  FeedBack=0;

float  KKp=1.5;  //比例常数  Proportional  Const 

float  KKi=0;

float  KKd=0.6;  //微分常数  Derivative  Const 

signed  int  EE0=0;  //当前误差 

signed  int  EE1=0;  //Error[-1] 

signed  int  EE2=0;  //Error[-2] 

signed  int  EError0=0,EError1=0;

signed  int  iiIncpid=0;

int  sp=0;

///////////////////////////////

///  舵机  PID  定义

////////////////////////////// 

float  Kp=3;  //比例常数  Proportional  Const 

float  Ki=1;

float  Kd=4.2;  //微分常数  Derivative  Const 

signed  int  E0=0  ;  //当前误差 

signed  int  E1=0;  //Error[-1] 

signed  int  E2=0;  //Error[-2]

signed  int  Error0=0,Error1=0;

signed  int  iIncpid=0;

/////////////////////////////////////

/////////////////////////////////////

////  电机  PID

/////////////////////////////////////

////////////////////////////////////

unsigned  int  Getspeed(int  cch1,int  cch2  ) 

{ 

EE0=cch1-cch2;  //增量计算 

EError0=EE0-EE1;

EError1=EE1-EE2;

iiIncpid=(int)(KKi*EError0+KKp*EE0+KKd*(EError0-EError1));

EE1=EE0; 

EE2=EE1;  //存储误差，用于下次计算 

sp+=iiIncpid;

if(sp>210)

sp=210;

if(sp<0)

sp=0;

return  sp;

}

void  sudu(int  SetPoint) 

{ 

pp=Getspeed(SetPoint,FeedBack);

PWMDTY2=pp;

}