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

#include /* common defines and macros */ #include "derivative.h" /* derivative-specific definitions */ #include #include #include #include #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; //设定目标 期望值 int FeedBack=0; float KKp=1.5; //比例常数 float KKi=0; float KKd=0.6; //微分常数 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; //比例常数 float Ki=1; float Kd=4.2; //微分常数 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; } ////////////////////////////// /////////////////////////////// /// 舵机 PID ////////////////////////////// ////////////////////////////////////// int Pcrtl(int ch1,int ch2 )

{ E0=ch1-0; //增量计算 Error0=E0-E1; Error1=E1-E2; iIncpid=(int)(Ki*Error0+Kp*E0+Kd*(Error0-Error1)); E1=E0; E2=E1; //存储误差，用于下次计算 return iIncpid; }

/////////////////////////////////////// void shache(int sha,int tim) { PWMDTY3=sha; time=tim; PITCE_PCE1=1; PITINTE_PINTE1=1; //启用中断通道0 zd=0; }

//////////////////////////////////////// void vIOPortInit(void) { DDRA=0X00; //PROTB 键盘输出 PORTA=0X00; DDRB=0xff; DDRJ=0xFF; //PROTB PTJ=0X80; }

void delayms(int ms) //40MHz--1ms { int ii,jj; if (ms<1) ms=1; for(ii=0;ii for(jj=0;jj<0xff;jj++); } ////////////////////////////////// void SetBusCLK_64M(void) { CLKSEL=0X00; //PLL系统脱离 PLLCTL_PLLON=1; //打开PLL SYNR =0xc0 | 0x07; REFDV=0xc0 | 0x01; POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz; _asm(nop); //BUS CLOCK=64M _asm(nop); while(!(CRGFLG_LOCK==1)); //当锁相环稳定时，使用它; CLKSEL_PLLSEL =1; //PLL系统进行; }

void ECT_Init(void) /*ECT初始化，使用输入捕捉功能*/ { TIOS =0x00; //设为输入捕捉 TSCR1=0x80; //定时器使能 TSCR2=0x07; TIE=0b00000010; //开中断 TCTL4=0b00001000; //0 ,1触发电平: //TCTL3=0b10101010;

} /////////////////////// void PWMInit(void) {

PWME=0x00; //禁止PWM输出

PWMPOL=0xff; PWMCLK=0x08; PWMCTL=0x10; PWMSCLB=80; PWMCAE=0x00; //左对齐 PWMPRCLK=0x44; //PWM时钟CLOCKA， PWMPER01=40000; PWMPER3=250; PWMPER2=255; PWMDTY2=0; PWMDTY3=31; PWMDTY01=4675; PWME=0x0f; /*使能01,23通道*/ } //////////////////////////

void SCI(void) { SCI1BDH=0X01; SCI1BDL=0XA0;

SCI1CR1=0X00; SCI1CR2=0X08; } void shacheV(int ch3)

{ PWMDTY3=ch3; while(FeedBack>30); PWMDTY3=30; zd=0; }

void TERMIO_PutChar(unsigned char ch) { while(!(SCI1SR1&0x80)) ; //非空时，继续等待 SCI1DRL=ch; }

unsigned char TERMIO_GetChar(void) { while(!(SCI1SR1&0x80)) ; //非空时，继续等待 return SCI1DRL; }

void un(unsigned char ch)

{

while(!(SCI1SR1&0X40)) { } SCI1DRL=ch; } ///////////////////////////// void ADInit(void) { ATD0CTL1=0x00; ATD0CTL2=0X40; /* 使能AD，清除标志 */ ATD0CTL3=0XB0; /* 队列长度为6，依次存入结果寄存器，继续转换 */ ATD0CTL4=0X05; /* 8位精度，总线频率32分频 */ ATD0CTL5=0X30; /* ，多通道，连续，从AD0开始转换*/ ATD0DIEN=0x00; //禁止数字输入

} //////////////////////////////// void PACN_init(void) { PACTL_PAEN=0; TSCR1=0X80; //快速清除flag：PAOVF PAIF TSCR2=0X03; PACNT=0; PACTL_PAEN=1; //启动脉冲累加器A PACTL_PAMOD=0; //事件计数方式 PACTL_PEDGE=1; //PT7引脚上的出现上升沿时脉冲累加器计数器加1 TSCR1_TEN =1; }

void Pit0_Init(void)