搜索
#include<hidef.h>/*commondefinesandmacros*/#include"derivative.h"/*derivative-specificdefinitions*/#include<math.h>typedefcharint8;#defineMAXCOUNTER1500#defineWBUFSIZE10#defineINVALIDSENSOR1000longcounter=0x00000000;//foroutofcourseintblackcount=0;//forcrosscoursesunsignedintsensor=0;intdelta=0,delta0=0;//¶æ»úƫת½Ç£¨0.1¶È£©intw[WBUFSIZE]={0,0,0,0,0,0,0,0,0,0};intw_p=0;//Æ«²î£¨mm£©»º³åÆ÷¼°ÆäÖ¸Õëintw1=0,w2=0,w3=0;//ƽ¾ùÆ«²î£¨mm£©intDT=1;//timeinterval(ms)intPulseNumAll=0;//×ÜÂö³åÊýintu;//longitudespeed(mm/s)//intpsi_r0=0,psi_r1=0;//psirequested(degree)intH=100,L=200;//HÊÇ´«¸ÐÆ÷µ½Ç°ÂÖ¾àÀë¡£LÊÇÇ°ºóÂÖ¾àÀëintrun_state=0;//stateofthecarintboma;intc=0;intc1=0;intc2=0;intc3=0;intdecv=0;/*******************PLL²¿·Ö***************************/voidSetBusCLK_64M(void)//OSC=16MHz{CLKSEL=0X00;//disengagePLLtosystemPLLCTL_PLLON=1;//turnonPLLSYNR=0xc0|0x07;REFDV=0x80|0x01;POSTDIV=0x00;//pllclock=2*osc*(1+SYNR)/(1+REFDV)=128MHz;//BUSCLOCK=pllclock/2=64MHz//fVCO=2*fOSC*(SYNDIV+1)/(REFDIV+1)//fPLL=fVCO/(2¡ÁPOSTDIV)//fBUS=fPLL/2//IfPOSTDIV=$00thenfPLLisidenticaltofVCO(dividebyone)._asm(nop);_asm(nop);while(!(CRGFLG_LOCK==1));//ʱÖÓУÕýͬ²½£¬whenpllissteady,thenuseit;CLKSEL_PLLSEL=1;//engagePLLtosystem;}/*************************PWM,I/O³õʼ»¯*******************************/voidPWM_Init(void){PWME=0x00;//PWMdisablePWMPOL=0xFF;//Êä³ö¿ªÊ¼Îª¸ßµçƽ,µ±¸ø¶¨¼ÆÊýµ½À´Ê±±äµÍµçƽPWMCLK=0x00;//PWM0,1ºÍPWM4,5ʱÖÓÔ´Ñ¡ÔñClockA;PWM2,3ʱÖÓÔ´Ñ¡ÔñClockB;PWMPRCLK=0x66;//ClockA=ClockB=64MHz/64=1MHzPWMCAE=0x00;//¶ÔÆ뷽ʽ£º×ó¶ÔÆë//PWMCTL_CON01=1;//PWM01ºÏ²¢16bitPWMPER0=200;//PWM1Period=200/1M=0.2ms,f=5kHZPWMDTY0=200;//Õ¼¿Õ±È=[(PWMDTYx+1)/(PWMPERx+1)]¡Á100%PWME_PWME0=1;//µç»úenablePWME_PWME1=0;PWMCTL_CON45=1;//PWM45ºÏ²¢16bitPWMPER45=20000;//PWM5Period=20000/1M=20ms.f=50HZPWMDTY45=1490;//Õ¼¿Õ±È=50%PWME_PWME5=1;//¶æ»úenable}/************Êý×ֶ˿ڳõʼ»¯*************/voidPort_Init(void){DDRA=0X00;//·½Ïò¿ØÖƼĴæÆ÷£¬ÉèÖÃA¿ÚÊäÈëDDR¼Ä´æÆ÷ÖÃ0ΪÊäÈ룬ÖÃ1ΪÊä³öPORTA=0X00;//Êý¾Ý¼Ä´æÆ÷£¬³õʼ»¯Ê±ÉèÖÃΪȫ0DDRB=0X00;//ͬÉÏPORTB=0X00;///////////////////////////////////////////////////////////////////DDRT=0x00;boma=PTIT&0x0f;}//TIMInputcaptureinitializevoidPT4_Capture_Init(void){TSCR1=0x80;//¶¨Ê±Æ÷ʹÄÜTIE=0x00;//ÿһλ¶ÔÓ¦ÏàӦͨµÀÖжϽûÖ¹,0±íʾ½ûÖ¹ÖжÏTCTL3=0x02;//EDGnBEDGnA1±íʾÉÏÉýÑØ,2±íʾϽµÑØ,3±íʾÈκÎÑØ//PT4ϽµÑØÓÐЧTIOS=0x00;//ÿһλ¶ÔӦͨµÀµÄ:0ÊäÈ벶׽,1Êä³ö±È½ÏTIE=0xff;//ÖжÏʹÄÜ}//PITinitialize,DTmsinterruptvoidPitInit(void){PITCFLMT_PITE=0;//disablePITPITCE_PCE0=1;//enabletimerchannel0PITMTLD0=64-1;//1MHzPITMUX=0X00;//ch0connectedtomicrotimer0PITLD0=DT*1000-1;//INTVERALmicrotimebases(PITMTLD+1)*(PITLD+1)/fBUSPITINTE_PINTE0=1;//enableinteruptchannel0PITCFLMT_PITE=1;//enablePIT//PITCNT0,ÀàËÆÓÚ×ÔÓɼÇÊýÆ÷,Ö»²»¹ýÒ»Ö±Êǵݼõ,TCNTÊÇÒ»Ö±µÝÔö}//¿ØÖÆÂí´ïËÙ¶Èvoidset_speed(void){if(counter<MAXCOUNTER){/////////////////ËÙ¶ÈÑ¡Ôñ///////////////////switch(boma){case0x00:PWMDTY0=120;break;case0x01:PWMDTY0=110+delta/30*delta/100;break;case0x02:PWMDTY0=100+delta/30*delta/100;break;case0x04:PWMDTY0=90+delta/30*delta/100;break;case0x08:PWMDTY0=80+delta/30*delta/100;break;case0x09:PWMDTY0=80;break;case0x03:PWMDTY0=70+delta/30*delta/100;break;case0x07:PWMDTY0=60+delta/30*delta/100;break;case0x0e:PWMDTY0=50+delta/30*delta/100;break;case0x0f:PWMDTY0=40+delta/30*delta/100;break;default:break;}/////////////////////////////////////////////////*if(boma)PWMDTY0=90;elsePWMDTY0=50+delta/30*delta/100;*/}else{PWMDTY0=200;}}//¶Á´«¸ÐÆ÷Öµvoidget_sensorvalue(void){sensor=PORTA|((PORTB&0X1F)<<8);sensor=~sensor;sensor=sensor&0x1fff;}//´«¸ÐÆ÷ֵת»»ÎªÆ«²îW(mm)//xxxxxxXxxxxxx//2109876543210intsensor_to_w(void){intw=INVALIDSENSOR+1;//invalidvalueswitch(sensor){case0x0001://bit0w=80;break;case0x0003://bit01w=70;break;case0x0002://bit1w=60;break;case0x0006://bit12w=50;break;case0x0004://bit2w=40;break;case0x000c://bit23w=35;break;case0x0008://bit3case0x001c://bit234w=30;break;case0x0018://bit34w=25;break;case0x0010://bit4case0x0038://bit345w=20;break;case0x0030://bit45w=15;break;case0x0020://bit5case0x0070://bit456w=10;break;case0x0060://bit56w=5;break;case0x0040://bit6case0x00e0://bit567w=0;break;case0x00c0://bit67w=-5;break;case0x0080://bit7case0x01c0://bit678w=-10;break;case0x0180://bit78w=-15;break;case0x0100://bit8case0x0380://bit789w=-20;break;case0x0300://bit89w=-25;break;case0x0200://bit9case0x0700://bit8910w=-30;break;case0x0600://bit910w=-35;break;case0x0400://bit10w=-40;break;case0x0c00://bit1011w=-50;break;case0x0800://bit11w=-60;break;case0x1800://bit1112w=-70;break;case0x1000://bit12w=-80;break;default:break;}//switch/*if(sensor==0x1fff){counter=0x00000000;blackcount=10;}*/if((sensor|~0x01f0)==0xffff)//ÅжÏÈý½Ç±êÖ¾{blackcount=10;++c1;if(c1>30&&c2==0){//trianglecounter=0;c1=0;c2=c2+1;}if(c1>30&&c2>0){counter=0;}}else{c1=0;}if(blackcount>0)--blackcount;if(counter<MAXCOUNTER){if(w<INVALIDSENSOR)counter=0x00000000;if((sensor&0x1800)&&(sensor&0x0003)//Twoendsisblack&&~(sensor|0x1803)&&!blac