freescalesmartcar.c

飞思卡尔智能小车程序,综和了前几届几个队伍的程序

      PORTB=0b11111011; 
     else if(max==4) 
      PORTB=0b11110111; 
     else if(max==5) 
      PORTB=0b11101111; 
     else if(max==6) 
      PORTB=0b11011111; 
     else if(max==7) 
      PORTB=0b10111111; 
     else if(max==8) 
      PORTB=0b01111111;  
     else  
    PORTB=0xff; 
    return max;  
 } 
 
 
  void Demarcate(void) //起跑前标定每个管子的最大值和最小值 
{ 
  unsigned int i; 
  GetADValue(); 
 //GetADValue1(); 
 //decrease(); 
  for(i=1;i<15;i++) 
  {     
    if(MaxAd[i]<AD_Data[i]) 
        MaxAd[i]=AD_Data[i]; 
    if(MinAd[i]>AD_Data[i])         MinAd[i]=AD_Data[i]; 
  } 
} 
 
 
void unitary(void){//计算percnet 值 
 unsigned int j; 
  for(j=1;j<15;j++) 
  { 
    if(MaxAd[j]<AD_Data[j])     
      AD_Data[j]=MaxAd[j];     
    if(MinAd[j]>AD_Data[j]) 
       AD_Data[j]=MinAd[j];   
   percent[j]=(unsigned char)((MaxAd[j]-AD_Data[j])*100/(MaxAd[j]-MinAd[j])); 
   // percent[j]=(unsigned char)((AD_Data[j]-MinAd[j])*100/(MaxAd[j]-MinAd[j])); 
   
  } 
} 
 
 //++ 
 
 
unsigned char first_checkAd() 
{   unsigned char i; 
    unsigned char black=0; 
    unsigned char j=FoundMax(); 
    nowposition=j; 
    if(percent[j]<60)  
    nowposition=preposition; 
   if(nowposition-preposition>6||preposition-nowposition>6) { 
     
     return 1; 
   } 
    
    for(i=1;i<=9;i++) 
    { 
      if(percent[i]>60) 
      { 
        black++; 
        if(black>1 && percent[i-1]<60) 
        return 1;                } 
    } 
      if(black>0 && black<=4) 
        return 0;  
      else  
        return 1; 
} 
 
 
 
//int weight[10]={0,-120,-80,-45,-17,0,17,45,80,120}; 
//int weight[10]={0,-240,-165,-115,-55,0,40,110,155,240} ; 
int weight[10]={0,-240,-180,-120,-60,0,60,120,180,240} ; 
unsigned char maxj;         
int FixDistance(void)   
{ 
  long temp1,temp2; 
  int position; 
  long tweight[3]; 
  unsigned char j=FoundMax(); 
  maxj=j; 
  if(percent[j]<60) 
    return pre_distance; 
  if(j==1)  
    position=-240; 
  else if(j==9) 
     position=240;  
 
   else{ 
     tweight[0]=weight[j]; 
     tweight[1]=weight[j+1]; 
     tweight[2]=weight[j-1]; 
     temp1=percent[j]*tweight[0]+percent[j+1]*tweight[1]+percent[j-1]*tweight[2] ; 
     temp2=percent[j]+percent[j+1]+percent[j-1]; 
     position=(int)(temp1/temp2);  
   } 
   return position; 
 
  } 
 
void angle_sample(void)   {      
    int pwmdty1=0; 
 
    //if(abs(distance)<10) 
    //pwmdty1=0; 
    //else { 
     if(distance<0) 
   pwmdty1=-fuzzy1(); 
      else 
   pwmdty1=fuzzy1(); 
    //} 
  
    if(pwmdty1>60) 
   pwmdty1=60; 
    if(pwmdty1<-58) 
   pwmdty1=-58; 
    PWMDTY23=ini_Angle+pwmdty1; 
   
 } 
  
  void diff1(void) 
{ 
   int j;    
   dif1=abs(distance)-error1[error1num-1]; 
   for(j=error1num-1;j>0;j--) 
      error1[j]=error1[j-1]; 
   error1[0]=abs(distance); 
    
} 
 
 
 
  
 void fix_distance(void) 
{    
    if(first_checkAd()==0) 
    { 
      distance=FixDistance(); 
       
    } 
  if(first_checkAd()==1) {     nowposition=preposition; 
      distance=pre_distance; 
  } 
   // dif_distance=diff1(); 
    angle_sample(); 
    pre_distance=distance; 
    preposition=nowposition; 
} 
 
 
 
 
  void GetADValue(void){ 
   int i; 
   ATD0CTL2_ADPU=1; 
   ATD1CTL2_ADPU=1; 
   for(i=0;i<1000;i++); 
    
    
    
    
   PTH=0b00100000; 
   PORTA=0b00000001; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=1; 
   ATD1CTL5_Cx=0;                   //Q1 
   while(!ATD0STAT0); 
   while(!ATD1STAT0); 
   AD_Data[1]=ATD0DR0L ; 
   AD_Data[8]=ATD1DR0L ; 
    
 
    
   PTH=0b00010000; 
   PORTA=0b00000010; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=2; 
   ATD1CTL5_Cx=1; 
   while(!ATD0STAT0); 
   while(!ATD1STAT0); 
   AD_Data[2]=ATD0DR0L ;    AD_Data[9]=ATD1DR0L ; 
    
    
    
    
   PTH=0b00001000; 
   PORTA=0b00100000; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=3; 
   ATD1CTL5_Cx=5; 
   while(!ATD0STAT0); 
   while(!ATD1STAT0); 
   AD_Data[3]=ATD0DR0L; 
   AD_Data[13]=ATD1DR0L; 
    
    
    
    
 
    
   PTH=0b00000100; 
   PORTA=0b00010000; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=4; 
   ATD1CTL5_Cx=4; 
   while(!ATD0STAT0); 
   while(!ATD1STAT0); 
   AD_Data[4]=ATD0DR0L; 
   AD_Data[12]=ATD1DR0L;  
    
    
    
   
   
    
   PTH=0b00000010; 
   PORTA=0b00000100; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=5; 
   ATD1CTL5_Cx=2; 
   while(!ATD0STAT0);    while(!ATD1STAT0); 
   AD_Data[5]=ATD0DR0L ; 
   AD_Data[10]=ATD1DR0L ; 
   
   
   
    
   PTH=0b00000001; 
   PORTA=0b01000000; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=6; 
   ATD1CTL5_Cx=6; 
   while(!ATD0STAT0); 
   while(!ATD1STAT0); 
   AD_Data[6]=ATD0DR0L; 
   AD_Data[14]=ATD1DR0L; 
   
   PTH=0b00000000; 
   
    
   
   
    
   PORTE=0b00001000; 
   PORTA=0b00001000; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=7; 
   ATD1CTL5_Cx=3; 
   while(!ATD0STAT0); 
   while(!ATD1STAT0); 
   AD_Data[7]=ATD0DR0L; 
   AD_Data[11]=ATD1DR0L; 
   PORTE=0b00000000; 
   PORTA=0b00000000; 
 
    
    
   
 
   
  //ATD0CTL2_ADPU=0;    while(!ATD1STAT0); 
   AD_Data[5]=ATD0DR0L ; 
   AD_Data[10]=ATD1DR0L ; 
   
   
   
    
   PTH=0b00000001; 
   PORTA=0b01000000; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=6; 
   ATD1CTL5_Cx=6; 
   while(!ATD0STAT0); 
   while(!ATD1STAT0); 
   AD_Data[6]=ATD0DR0L; 
   AD_Data[14]=ATD1DR0L; 
   
   PTH=0b00000000; 
   
    
   
   
    
   PORTE=0b00001000; 
   PORTA=0b00001000; 
   for(i=0;i<N;i++); 
   ATD0CTL5_Cx=7; 
   ATD1CTL5_Cx=3; 
   while(!ATD0STAT0); 
   while(!ATD1STAT0); 
   AD_Data[7]=ATD0DR0L; 
   AD_Data[11]=ATD1DR0L; 
   PORTE=0b00000000; 
   PORTA=0b00000000; 
 
    
    
   
 
   
  //ATD0CTL2_ADPU=0;   // ATD1CTL2_ADPU=0; 
  // for(i=0;i<=1000;i++);  
} 
 
 
    void main(void) { 
    /* put your own code here */ 
  int i; 
  DDRA=0xff;   //PORTA,H,B out 
  DDRH=0x3f; 
  DDRE=0x08; 
  DDRB=0xff;    //PTJ,PTS in 
  DDRJ=0x00; 
  DDRS=0x00; 
  PORTA=0; 
  PORTE=0; 
  PTH=0; 
 
 initiaPLL(); 
 initialSCI(); 
 initialATD(); 
 intialPWM(); 
 initialECT(); 
 PIDInit(); 
 EnableInterrupts; 
 
   
  for(i=1;i<15;i++) 
 { 
  MaxAd[i]=0; 
  AD_Data[i]=0 ; 
  MinAd[i]=255; 
  percent[i]=0; 
 } 
while(PTS_PTS2==1); 
delay(2);  
while(PTS_PTS2==0) 
  {    
   
    Demarcate();   
  }  
delay(1);      
  
 if(PTJ_PTJ6==0)PWMDTY23=500; 
 else if(PTJ_PTJ7==0)PWMDTY23=490; 
 else if(PTS_PTS7==0)PWMDTY23=480; 
 else if(PTS_PTS6==0)PWMDTY23=475; 
 else if(PTS_PTS5==0)PWMDTY23=470; 
 else PWMDTY23=600; 
  
 delay(1); 
  
  for(i=0;i<5;i++) 
  output_mfs[i]=speed[speedlevel][i]; 
 
  
 GetADValue(); 
  
 //unitary(); 
 //fix_distance(); 
 TSCR1_TEN = 1;    
                             
  
 for(;;) { 
  
  
 
 GetADValue(); 
                   
   
  
  //unitary(); 
 
 
// fix_distance(); 
  CheckStart(); 
 PWMDTY01=500; 
   
 } 
} 
 
#pragma CODE_SEG __NEAR_SEG NON_BANKED  
void interrupt 10 TimerC2I(void)      //通道 2 定时器中断 
{ TC2+=2500; 
  TFLG1_C2F = 1;           
  sPID.vi_FeedBack=PACN10; 
  PACN10=0; 
  printcount++; 
}  
 
 
void interrupt 9 TimerC1I(void){        //通道 1 定时器中断 
TFLG1_C1F=1; 
if(isstart>=2&&iscrossing==0)startline++; 
iscrossing=0; 
isstart=0; 
flag=0; 
blackflag=0; 
TIE_C1I=0; 
PORTB=0xff; 
}  
   
#pragma CODE_SEG DEFAULT