小车直立程序

include /* common defines and macros */#include /* derivative information */#include #include #include #pragma LINK_INFO DERIVATIVE "mc9s12xs128" uchar i=0; char k,z=0; float fP, fI; void delay(unsigned int h) { unsigned int j,tk; for(tk=0;tk

PITTIME*0.01MSPITINTE_PINTE0=1;// 定时器中断通道 0 中断使能PITCFLMT_PITE=1;// 定时器通道 0 使能}//ad初始化void ADC_Init(void){ //printf("ADC_Init\n"); ATD0CTL1=0x40; //7:1-外部触发,65:00-8位精度,4:放电,3210:ch ATD0CTL2=0x40; //禁止外部触发, 中断禁止 ATD0CTL3=0xC0; //右对齐无符号,每次转换8个序列, No FIFO, Freeze模式下继续转 ATD0CTL4=0x05; //765:采样时间为4个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1] ATD0CTL5=0x20; //6:0特殊通道禁止,5:1连续转换 ,4:1多通道轮流采样 ATD0DIEN=0x00; //禁止数字输入 }//pwm初始化void PWM() { PWMPRCLK=0X11; //对总线时钟进行预分频，总线时钟为 8M，分频后为500K PWMCLK=0x0f; //设 sA sb为其时钟源 PWMSCLA=0X05; PWMSCLB=0X05; //A时钟为 2000Hz PWMPOL=0X00;//上升沿翻转 PWMCAE=0X00;//左对齐输出 PWMPER0=0XF0; PWMPER1=0XF0; PWMPER2=0XF0; PWMPER3=0XF0; PWMCNT0=0X00;//0通道计数器清 0 PWMCNT2=0X00; }int getad(char m) { int ad; ATD0CTL5=(0X20+m); while(!ATD0STAT0_SCF); switch(m) { case 0:ad=ATD0DR0; case 1:ad=ATD0DR1; case 2:ad=ATD0DR2; case 3:ad=ATD0DR3; case 4:ad=ATD0DR4; } return ad;}float filter(char n){ float sum = 0; char count; int jieguo; for ( count=0;count<10;count++) { jieguo= getad(n); sum = sum+jieguo; asm NOP; asm NOP; } sum=sum/10; return sum;}void SpeedControl(void) { float fDelta; int g_fCarSpeed; g_fCarSpeed=PACNT; PACNT=0X0000; g_fCarSpeed =g_fCarSpeed; fDelta =0-g_fCarSpeed; fP = fDelta*1.2; //SPEED_CONTROL_P; fI = fDelta*0.001;// SPEED_CONTROL_I; }void PACAInit(){ PACTL=0X50; PACNT=0X0000;}void main(){ float gry=0,gav=0,gry1,gry11; float carangle=2,gavangle,pouty,poutz; float p,h; int dianjiy,dianjiz,g_nSpeedControlCount=0; int zuo,you,cha; DDRB=0XFD; PORTB=0X00; PORTB_PB0=1; pllclk(); initPIT(); ADC_Init(); PACAInit(); PWM(); PWME=0x00; if(PORTB_PB1==0) gry1=1185; else gry1=1200; //delay(3000); //gry1=filter(2); //gry1=filter(2)+gry1; //gry1=filter(2)+gry1; //gry1=filter(2)+gry1; //gry1=filter(2)+gry1; //gry1=filter(2)+gry1; //gry1=gry1/6; gry11=filter(0); gry11=filter(0)+gry11; gry11=filter(0)+gry11; gry11=filter(0)+gry11; gry11=filter(0)+gry11; gry11=filter(0)+gry11; gry11=gry11/6; delay(1000); EnableInterrupts; for(;;) {if(z==1) { zuo=filter(2); you=filter(3); cha=you-zuo; cha=cha/4; z=0; } if(i==1) { gry=filter(0); gry=gry11-gry; gry=gry*0.42; gav=filter(1); gav=gry1-gav; gavangle=gav/7-fI; //jiaodu=(gavangle-carangle)/4; //carangle=carangle+(gry+jiaodu)/200; carangle=carangle+gry/200; p=p+0.0025; h=p/(p+60); carangle=carangle+h*(gavangle-carangle); p=(1-h)*p; //if(carangle>0) // PORTB_PB3=1; //else //PORTB_PB3=0; g_nSpeedControlCount ++; if(g_nSpeedControlCount>=20) { SpeedControl(); g_nSpeedControlCount = 0; } pouty=carangle*32+gry*1.4-fP-cha;poutz=carangle*32+gry*1.4-fP+cha;dianjiy=(int)pouty; dianjiz=(int)poutz;PWME=0x00; if(dianjiy>0) { k=1; if(carangle>30) dianjiy=0; if(dianjiy>240) dianjiy=240; PWMDTY1=240-dianjiy; PWMDTY0=240; } else { k=-1; if(carangle<-30) dianjiy=0; if(dianjiy<-240) dianjiy=-240; PWMDTY1=240; PWMDTY0=240+dianjiy; } if(dianjiz>0) { k=1; if(carangle>30) dianjiz=0; if(dianjiz>240) dianjiz=240; PWMDTY2=240-dianjiz; PWMDTY3=240; } else { k=-1; if(carangle<-30) dianjiz=0; if(dianjiz<-240) dianjiz=-240; PWMDTY2=240; PWMDTY3=240+dianjiz; } PWME=0X0f; i=0; } } }#pragma CODE_SEG __NEAR_SEG NON_BANKED void interrupt 66 PIT0(void) {i++ ;z++;PITTF_PTF0=1;}。