【正文】 dMax()。 maxj=j。 if(j==1) position=200。 else if(j==9) position=200。 else{ if(abs(percent[j+1]percent[j1])10) { tweight[0]=weight[j]。 tweight[1]=weight[j+1]。 tweight[2]=weight[j1]。 temp1=percent[j]*tweight[0]+percent[j+1]*tweight[1]+percent[j1]*tweight[2] 。 temp2=percent[j]+percent[j+1]+percent[j1]。 position=(int)(temp1/temp2)。 } else { if(percent[j+1]percent[j1]) { tweight[0]=weight[j]。 tweight[1]=weight[j+1]。 temp1=percent[j]*tweight[0]+percent[j+1]*tweight[1]。 temp2=percent[j]+percent[j+1]。 position=(int)(temp1/temp2)。 } else { tweight[0]=weight[j]。 tweight[1]=weight[j1]。 temp1=percent[j]*tweight[0]+percent[j1]*tweight[1]。 temp2=percent[j]+percent[j1]。 position=(int)(temp1/temp2)。 } } } return position。 } void angle_sample(void) { int pwmdty1=0。 pwmdty1=(distance/4+dif_distance/3)。 if(pwmdty155) pwmdty1=55。 if(pwmdty155) pwmdty1=55。 PWMDTY01=ini_Angle+pwmdty1。 } int diff(void){ int j。 dif=distanceerror[LAST_ERROR_NUM1]。 for(j=LAST_ERROR_NUM1。j0。j) error[j]=error[j1]。 error[0]=distance。 return dif。} void diff1(void){ int j。 dif1=abs(distance)error1[error1num1]。 for(j=error1num1。j0。j) error1[j]=error1[j1]。 error1[0]=abs(distance)。} void fix_distance(void){ if(first_checkAd()==0) { distance=FixDistance()。 } if(first_checkAd()==1) { nowposition=preposition。 distance=pre_distance。 } if(first_checkAd()==2) { nowposition=preposition。 distance=FixDistance1()。 } dif_distance=diff()。 diff1()。 angle_sample()。 pre_distance=distance。 preposition=nowposition。} void GetADValue(void){ int i。 ATD0CTL2_ADPU=1。 //發(fā)生ATD0中斷 ATD1CTL2_ADPU=1。 //發(fā)生ATD1中斷 for(i=0。i1000。i++)。 PTH_PTH0=0。 PTH_PTH1=0。 //A0 A1 A2 A3 A4 A5 A6 A7 H0 PTH_PTH2=0。 //0 1 2 3 4 5 6 7 00 PTH_PTH3=0。 // H1 H2 H3 H4 H5 PTH_PTH4=0。 // 01 02 03 04 05 PTH_PTH5=0。 PORTA=0b00000001。 for(i=0。iN。i++)。 ATD0CTL5_Cx=0。 while(!ATD0STAT0)。 AD_Data[1]=ATD0DR0L 。 PORTA=0b00000010。 for(i=0。iN。i++)。 ATD0CTL5_Cx=1。 while(!ATD0STAT0)。 AD_Data[2]=ATD0DR0L 。 PORTA=0b00000100。 for(i=0。iN。i++)。 ATD0CTL5_Cx=2。 while(!ATD0STAT0)。 AD_Data[3]=ATD0DR0L 。 PORTA=0b00001000。 for(i=0。iN。i++)。 ATD0CTL5_Cx=3。 while(!ATD0STAT0)。 AD_Data[4]=ATD0DR0L 。 PORTA=0b00010000。 for(i=0。iN。i++)。 //N=250 延時(shí) ATD0CTL5_Cx=4。 //cx=1表示ATD0的通道0 while(!ATD0STAT0)。 AD_Data[5]=ATD0DR0L 。 PORTA=0b00100000。 for(i=0。iN。i++)。 ATD0CTL5_Cx=5。 while(!ATD0STAT0)。 AD_Data[6]=ATD0DR0L。 PORTA=0b01000000。 for(i=0。iN。i++)。 ATD0CTL5_Cx=6。 while(!ATD0STAT0)。 AD_Data[7]=ATD0DR0L。 PORTA=0b10000000。 for(i=0。iN。i++)。 ATD0CTL5_Cx=7。 while(!ATD0STAT0)。 AD_Data[8]=ATD0DR0L。 PORTA=0b00000000。 PTH_PTH0=1。 for(i=0。iN。i++)。 //N=250 延時(shí) ATD1CTL5_Cx=0。 //cx=0表示ATD1的通道0 while(!ATD1STAT0)。 //判斷AD轉(zhuǎn)換完成 （！ATD0START0=0)則跳出while AD_Data[9]=ATD1DR0L 。 PTH_PTH0=0。 PTH_PTH1=1。 for(i=0。iN1。i++)。 ATD1CTL5_Cx=1。 while(!ATD1STAT0)。 AD_Data[10]=ATD1DR0L。 PTH_PTH1=0。 PTH_PTH2=1。 for(i=0。iN1。i++)。 ATD1CTL5_Cx=2。 while(!ATD1STAT0)。 AD_Data[11]=ATD1DR0L 。 PTH_PTH2=0。 PTH_PTH3=1。 for(i=0。iN1。i++)。 ATD1CTL5_Cx=3。 while(!ATD1STAT0)。 AD_Data[12]=ATD1DR0L 。 PTH_PTH3=0。 PTH_PTH4=1。 for(i=0。iN1。i++)。 ATD1CTL5_Cx=4。 while(!ATD1STAT0)。 AD_Data[13]=ATD1DR0L 。 PTH_PTH4=0。 PTH_PTH5=1。ATD1CTL5_Cx=5。 //加速度傳感器X軸采樣 while(!ATD1STAT0)。 AD_Data[14]=ATD1DR0L 。 PTH_PTH5=0。 PTH_PTH6=1。 for(i=0。iN1。i++)。 ATD1CTL5_Cx=6。 //加速度傳感器Y軸采樣 while(!ATD1STAT0)。 AD_Data[15]=ATD1DR0L 。 PTH_PTH6=0。 PTH_PTH7=1。 for(i=0。iN1。i++)。 ATD1CTL5_Cx=7。 //加速度傳感器Z軸采樣 while(!ATD1STAT0)。 AD_Data[16]=ATD1DR0L 。 PTH_PTH7=0。 ATD0CTL2_ADPU=0。 ATD1CTL2_ADPU=0。 for(i=0。i=1000。i++)。 }void init_firstspeed(void){ int i。 if(PORTB_BIT2==1amp。amp。PORTB_BIT1==1amp。amp。PORTB_BIT0==0)speedlevel=1。 else if(PORTB_BIT2==1amp。amp。PORTB_BIT1==0amp。amp。PORTB_BIT0==1) speedlevel=2。 else if(PORTB_BIT2==1amp。amp。PORTB_BIT1==0amp。amp。PORTB_BIT0==0) speedlevel=3。 else if(PORTB_BIT2==0amp。amp。PORTB_BIT1==1amp。amp。PORTB_BIT0==1) speedlevel=4。 else if(PORTB_BIT2==0amp。amp。PORTB_BIT1==1amp。amp。PORTB_BIT0==0) speedlevel=5。 else if(PORTB_BIT2==0amp。amp。PORTB_BIT1==0amp。amp。PORTB_BIT0==1) speedlevel=6。 else if(PORTB_BIT2==0amp。amp。PORTB_BIT1==0amp。amp。PORTB_BIT0==0) speedlevel=7。 else speedlevel=0。 for(i=0。i5。i++) { output_mfs[i]=speed[speedlevel][i]。 }} void main(void) { int i。 DDRA=0xff。 DDRH=0xff。 DDRS=0xff。 PTS=0x70。 DDRB=0x00。 initiaPLL()。 initialSCI()。 initialATD()。 intialPWM()。 initialECT()。Init_io()； PIDInit()。 EnableInterrupts。 TSCR1_TEN = 1。 init_speed=200。 for(i=1。i15。i++) { MaxAd[i]=0。 AD_Data[i]=0 。 MinAd[i]=255。 percent[i]=0。 } while(PORTB_BIT7==1)。 delay(2)。 while(PORTB_BIT7==0) { Demarcate()。 } init_firstspeed()。 GetADValue()。 unitary()。 fix_distance()。 for(。) { GetADValue()。 unitary()。 fix_distance()。 CheckStart()。 i