main.c

飞思卡尔智能车竞赛程序飞思卡尔智能车竞赛程序飞思卡尔智能车竞赛程序

             
  
    */
  errTop=VIDEO_CENTER_TOP-ImagePar[top];
  LastErr=err;
 
 //err=VIDEO_CENTER-ImagePar[SAMPLELINE-1];
  if (top<2)
    err=VIDEO_CENTER-ImagePar[SAMPLELINE/2]; //di yi hang
  
  else
    if (ImageGood[SAMPLELINE/2+top/2]==1)
     err=VIDEO_CENTER-ImagePar[SAMPLELINE/2+top/2];
    else {
       err=VIDEO_CENTER-ImagePar[SAMPLELINE-1];
     }
  
   return 1 ;    
}
  
/*******************************************/

 
unsigned char  scan(register unsigned char *p){				 //从左向右扫描，获取一行中黑线的位置
 unsigned char j=0;
 unsigned char L=0;
 char dev;
 j=VIDEO_LEFT ;
while(j<SAMPLENUM-1){
   		  dev=*(p+j)-*(p+j+3);
        if (dev>VALVE)
            L=j;
        else
          if(-dev>VALVE)
            if ((j-L)<10&&L>0)
            return (j+L)/2+2;
      j++;
 }
       return 0;   
}
/*********************scan from the right ***********************/
unsigned char scan2(register unsigned char *p){				 //从右向左扫描，获取一行中黑线的位置
 unsigned char i=0;
 unsigned char L=0;
 char dev;
 
 i=SAMPLENUM-1 ;
while(i>VIDEO_LEFT-1){
   		  dev=*(p+i)-*(p+i-3);
        if (dev>VALVE)
            L=i;
        else
          if(-dev>VALVE)
            if ((i-L)<10&&L>0)
            return (i+L)/2-1;
      i--;
 }
       return 0;
}
/****************PP1:OUTPUT OF SERVO PWM SIGNAL****************************/
void Init_servo(void){
  servoScale=SERVO_CENTER; 
  err=0;
  LastErr=0;
  Last_servoScale=SERVO_CENTER;
  PWMSCLA=5;  //SA=40M/(5*2)=4M
 
  PWMCLK|=0X03; //clock source from clock sa
  PWMPRCLK|=0X02;  //prescale clock SA to 1mhz
 
  
  
  PWMCTL=0X10; //CONNECT CHANEL0 AND CHANEL1
  PWMPER01=10000; //PRE 10MS
  PWMDTY01=servoScale;  
  PWMPOL|=0X02;
  PWME|=0X02; //ENABLE PWM0
}
/*************************************************************/
void Init_motor(void){

  
 DDRA=0Xc0 ;//PORTA_BIT7,PORTA_BIT6 OUT
  PORTA_BIT7=1; // D2
  PORTA_BIT6=0; //D1
 // PORTA_BIT5=0; //IN2
  
  PWMSCLB=5;  //SB=40M/(5*2)=4M
   // PWMSCLB=4; 
  //IN1
  PWMCLK|=0X04; //clock source from clock sb
  PWMPRCLK|=0X20; //prescale clock sb to 1mhz
  PWMPER2=100;
  PWMDTY2=30; 
  PWMPOL|=0X04;
  //IN2
 
  PWMCLK|=0X08;
  PWMPER3=100;
  PWMDTY3=0;
  PWMPOL|=0X08;
    
  PWME|=0X08;
  PWME|=0X04 ; //ENABLE PWM2 PP2 
}
/**************************************************************/
void Car_control(void)
{
Image_good=GetLine();
  if(Image_good){
  #ifdef _AUTOSTOP
   errCount=0; 
 //  PORTB_BIT7=1;
   #endif
   PORTB=0XFF;
  Last_servoScale=servoScale;               //err=VIDEO_CENTER-ImagePar
  if (top<2) {
     if (err<15&&err>-15)
       servoScale=SERVO_CENTER+err*PROPOTION0-(err-LastErr)*2;
     else
       servoScale=SERVO_CENTER+err*PROPOTION1+(err-LastErr)*2;;
  // PORTB=0X00;
  }
  else {
    servoScale=SERVO_CENTER+err*PROPOTION2+(err-LastErr)*DERIVATIVE;
  //  servoScale=SERVO_CENTER+err*P_Tab[SAMPLELINE/2+top/2]+(err-LastErr)*DERIVATIVE;
    //PORTB=0XFF;
  }
    /*
  if (servoScale<=(SERVO_CENTER-100)&&Last_servoScale>=(SERVO_CENTER+100)) {
   servoScale=Last_servoScale;
    err=LastErr;
  } else
    if (servoScale>(SERVO_CENTER+100)&&Last_servoScale <(SERVO_CENTER-100)) {
    servoScale=Last_servoScale;
    err=LastErr;
    }
  */   
	 if (servoScale>SERVO_LEFT) servoScale=SERVO_LEFT;
	 if (servoScale<SERVO_RIGHT) servoScale=SERVO_RIGHT;
   
  } else {
    PORTB=0X00;
 if ((servoScale>SERVO_CENTER+100||servoScale<SERVO_CENTER-100)&&(cur>5||cur<-5)){
    if (servoScale<SERVO_CENTER)
      servoScale=SERVO_RIGHT;
    if(servoScale>SERVO_CENTER )
      servoScale=SERVO_LEFT;
      }
      
  #ifdef _AUTOSTOP
    errCount++;
    //PORTB_BIT7=0 ;
    if (errCount>100) {
      PORTA_BIT7=0;    //stop the car 
      PORTA_BIT6=1;
      //DDRB=0XFF;
      //PORTB=0X00;
    }
    #endif
  }

  #ifdef _REGULATER
    #ifdef _CENTER 
     PWMDTY01=SERVO_CENTER;
    #endif
     
     #ifdef _LEFT
        PWMDTY01=SERVO_LEFT;
     #endif
     
     #ifdef _RIGHT
           PWMDTY01=SERVO_RIGHT;
     #endif 
  #else  
  PWMDTY01=servoScale;
  #endif 
}
/******************************************************************/
void  filter(void) {
/*
  char i=1;
  while(i<SAMPLELINE-1) {
   if (ImagePar[i]!=0&&ImagePar[i-1]==0&&ImagePar[i+1]==0) {
    ImagePar[i]=0;
    ImageGood[i]=0;
   }
   i++;
  }
  */
  char i=SAMPLELINE-1;
  unsigned char LastFine=0;
  unsigned char skip=0;
  while (ImageGood[i]==0) {
    i--;
  skip++;
  }
    if (skip<5)
     LastFine=ImagePar[i--];
  while (i>=0&&skip<5) {
    
     if (ImagePar[i]>0) {
      
         if (ImagePar[i]>LastFine+20) {
          ImagePar[i]=0;
          ImageGood[i]=0;
         skip++;
         }
         else
           if (ImagePar[i]<LastFine-20) {
            ImagePar[i]=0;
            ImageGood[i]=0;
           skip++;
           }
            else {
              LastFine=ImagePar[i];
             skip=0;
            }
     } 
     
     i--;
  }
  while( i>=0) {
    ImagePar[i]=0;
    ImageGood[i--]=0;
  }
}
/********************************************/
//轮子周长17CM ,43齿  ,PIN PT7
void Speed_init(void) {
  PACTL_PAEN=1;
  PACTL_PAMOD=0; //PULSE ACCUMULATOR MODE
  PACTL_PEDGE=1; //COUNT ON RISING EDGE
  
}
/*******************************************/


void Speed_Control(void){     
       unsigned char Speed_Index;
       find_Cur();
     
       //  PORTB=0X00;

       if (cur<0)
          Speed_Index=-cur;
       else

        Speed_Index=cur;
      
       if (ON_THE_LINE&&Speed_Index>1) {
        if (Sample_speed>10) {
          Set_speed=5;
        PORTB=0X00;
        }
        else
           if (cur<-4)
            Set_speed= Speed_Tab[Speed_Index]-2;
           else
            Set_speed=Speed_Tab[Speed_Index];
            PORTB=0Xff;
       } 
        else {
         if (cur<-4)  //  turning   right
          Set_speed=Speed_Tab[Speed_Index]-2;
         else
          Set_speed=Speed_Tab[Speed_Index]; 
          PORTB=0Xff;
      }
    
      // Set_speed= Speed_Tab[Speed_Index];
       
        err_speed=(Set_speed-Sample_speed);
          
      //   if (err_speed<7&&err_speed>-7)       //2
        //   Speed+=err_speed*10;         //10
         //if((err_speed>=7&&err_speed<19)||(err_speed<=-7&&err_speed>-19))  //2,10
           // Speed+=err_speed*15;          //15
         
         //if (err_speed>=19)    //10
           //    Speed+=50;      //50
         //if(err_speed<=-19)
           //  Speed-=50;
             
             
         if (err_speed<0) {
            if (err_speed>-4)       //2
               Speed+=err_speed*10;         //10
            if(err_speed<=-4&&err_speed>-10)  //2,10
               Speed+=err_speed*25;          //15
               
            if(err_speed<=-10)
               Speed-=50;//Speed-=50;
         }else {
            if (err_speed<7)       //2
               Speed+=err_speed*5;         //10
            if(err_speed>=7&&err_speed<19)  //2,10
               Speed+=err_speed*10;          //15
            if (err_speed>=19)    //10
               Speed+=50;      //50
           }
               
         if (Speed<0)
          Speed=0;
         if (Speed>100)
          Speed=100;
        
         PWMDTY2=Speed;
         //CLEAR FLACT STATUS FLAG
          if (PORTA_BIT4==0||PORTA_BIT5==0) {
                PORTA_BIT7=0;
                PORTA_BIT6=1;
                //PWMDTY2=50;
                PORTA_BIT7=1; // D2
                PORTA_BIT6=0; //D1
         }
     
          
  //       if (Speed==100)
         //PORTB_BIT6=0;
//         else
         //PORTB_BIT6=1;      
 

         
      
          
}

/*****************************************/
void find_Cur(void) {
  if (Image_good) {
    if (top!=0) {     
      if (errTop<0) //errtop=VIDEO_CENTER_TOP-ImagePar
        cur=0-top-3; //向右弯
      else
        cur=top+3;   //向左弯
     }
    else {
       cur=(errTop)/20;
      }
  }// else {
    //if (cur<0)
     // cur=-22;
    //if (cur>0)
      // cur=22;
  //}
  /*
 if (cur<0)
    PORTB=~(-cur);
 else
    PORTB=~cur;
    */
 if (cur<1&&cur>-1)
   ON_THE_LINE=1;
 if (cur>4||cur<-4)
   ON_THE_LINE=0;
 return; 
}

/******************************************/
unsigned char find_S(void) {
  unsigned char i;
  for (i=2;i< SAMPLELINE-3;i++)
     if ((ImagePar[i]>ImagePar[i-2]&&ImagePar[i]>ImagePar[i+2])||(ImagePar[i]<ImagePar[i-2]&&ImagePar[i]<ImagePar[i+2]))
        return 1;
   return 0;
   
  
}
/******************************************/
void sci_init(void) {
SCI0CR2=0x0c;
SCI0BDH=0x00;
SCI0BDL=130;  //baul 19200 at 40m clock  
}
/******************************************/
void sci_send(unsigned char ch)
{
while(!(SCI0SR1&0x80))
{}         
SCI0DRL=ch;
}
/******************************************/
void send(unsigned char  *p) {
  unsigned int i;
   for (i=0;i<SAMPLELINE*SAMPLENUM;i++)
      sci_send(*p++);
          
}