#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 3unsigned 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 Valueint FeedBack=0;float KKp=1.5; //比例常数 Proportional Constfloat KKi=0;float KKd=0.6; //微分常数 Derivative Constsigned 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 Constfloat Ki=1;float Kd=4.2; //微分常数 Derivative Constsigned 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));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));E2=E1; //存储误差，用于下次计算return iIncpid;}///////////////////////////////////////void shache(int sha,int tim){PWMDTY3=sha;time=tim;PITCE_PCE1=1;PITINTE_PINTE1=1; //enable interupt channel 0 zd=0;}////////////////////////////////////////void vIOPortInit(void){DDRA=0X00; //PROT B 键盘输出PORTA=0X00;DDRB=0xff;DDRJ=0xFF; //PROTBPTJ=0X80;}void delayms(int ms) //40MHz--1msint ii,jj;if (ms<1) ms=1;for(ii=0;ii<ms;ii++)for(jj=0;jj<0xff;jj++);}//////////////////////////////////void SetBusCLK_64M(void){CLKSEL=0X00; //disengage PLL to system PLLCTL_PLLON=1; //turn on PLLSYNR =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)); //when pll is steady ,then use it; CLKSEL_PLLSEL =1; //engage PLL to system; }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)) ; //keep waiting when not empty SCI1DRL=ch;}unsigned char TERMIO_GetChar(void){while(!(SCI1SR1&0x80)) ; //keep waiting when not empty 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 PAIFTSCR2=0X03;PACNT=0;PACTL_PAEN=1; //启动脉冲累加器APACTL_PAMOD=0; //事件计数方式PACTL_PEDGE=1; //PT7引脚上的出现上升沿时脉冲累加器计数器加1 TSCR1_TEN =1;}void Pit0_Init(void)PITCFLMT_PITE=0; //disable PITPITCE_PCE0=1; //enable timer channel 0PITMUX_PMUX0=0; //ch0 connected to micro timer 0PITMTLD0=0X0F ; //micro time base 0 equals 255 clock cyclesPITLD0=0XFFFF; //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS. //时间计算//For example, for a 16 MHz bus clock, the maximum time-out period equals://15 * 65536 * /64000000 = 0.01536s.PITINTE_PINTE0=1; //enable interupt channel 0PITCFLMT_PITE=1; //enable PIT}void Pit1_Init(void){PITCFLMT_PITE=0; //disable PIT//PITCE_PCE1=1; //enable timer channel 0PITMUX_PMUX1=0; //ch0 connected to micro timer 0PITMTLD1=0X0F ; //micro time base 0 equals 255 clock cyclesPITLD1=0XFFFF; //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS. //时间计算//For example, for a 16 MHz bus clock, the maximum time-out period equals://15 * 65536 * /64000000 = 0.01536s.//PITINTE_PINTE1=1; //enable interupt channel 0PITCFLMT_PITE=1; //enable PIT}/////////////////////////////////////void bustor(int x[],int n)int i,j,m,k;for(i=1;i<=n-1;i++) {k=1;for(j=1;j<=n-i;j++) if(x[j]>x[j+1]){m=x[j];x[j]=x[j+1];x[j+1]=m;k=0;}if(k==1)break;}}/////////////////////////// void kongzhi(void){vIOPortInit() ;SetBusCLK_64M() ; ADInit() ;SCI();PORTB=0X00;PWMInit();PACN_init();Pit0_Init();Pit1_Init();for(;;){ECT_Init();while(!ATD0STAT0_SCF); /*等待当前队列转换完成*/b=ATD0DR0L;c=ATD0DR1L;d=ATD0DR2L;e=ATD0DR3L;f=ATD0DR4L;g=ATD0DR5L;//printf("x1=%d x2=%d FeedBack=%d EE0=%d iiIncpid=%d sp=%d b=%d c=%d ",x1,x2,FeedBack,EE0,iiIncpid,sp,b,c) ;if(b>0x10&&c>0x10){x1=b-210;x2=c-210;if(d<e)x1=-x1;if(f<g)x2=-x2;y=Pcrtl(x1,x2);if(y>560)y=560;if(y<-450)y=-450;PWMDTY01=4675-y;//printf("x1=%d x2=%d",x1,x2);if(x2>80||x2<-80) //大弯道 {if(zd==1&&FeedBack>78) {sudu(65);if(d>e){PORTB=0XF0;shacheV(8);}else{PORTB=0X0F;shacheV(52);}}}else //直道{if(x1>50||x1<-50){x4++;if(x4<60)sudu(60);elsesudu(60);}else{zd=1;sudu(100);PWMDTY3=30;x4=0;}}}elsesudu(65);}}void main(void) {/* put your own code here */EnableInterrupts;kongzhi();for(;;) {_FEED_COP(); /* feeds the dog */} /* loop forever *//* please make sure that you never leave main */}///////////////////////////////////#pragma CODE_SEG __NEAR_SEG NON_BANKEDvoid interrupt 9 T1CaptureInterrupt(void){ TFLG1=0b00000010;i++;PORTB=0xff-i;un(i);if(i>0x0c){PTJ=0X00;}}#pragma CODE_SEG DEFAULT//////////////////////////////////////////////////////////////////////////#pragma CODE_SEG __NEAR_SEG NON_BANKEDvoid interrupt 66 PIT0_ISR(void){PITTF_PTF0=1; //CLER FLAGFeedBack=PACNT;PACNT=0; }//////////////////////////////////#pragma CODE_SEG __NEAR_SEG NON_BANKEDvoid interrupt 67 PIT1_ISR(void){PITTF_PTF1=1; //CLER FLAGr++;if(r==time){ r=0;zd=0;PORTB=0XFF;PWMDTY3=30;PITCE_PCE1=0;PITINTE_PINTE1=0; //DISable interupt channel 0 }}。
