main.c

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

#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#include "../../header/s12/s12.h"

#pragma LINK_INFO DERIVATIVE "mc9s12dg128b	"

#include "main_asm.h" /* interface to the assembly module */


#define PING 1
#define PONG 0
#define SAMPLENUM 93 //97
#define VIDEO_LEFT   8
#define VIDEO_RIGHT 92
#define SAMPLELINE 15
#define TOTALNUM 1164
#define VALVE 17
#define VIDEO_CENTER 52
#define VIDEO_CENTER_TOP 52
#define SERVO_CENTER 1510
#define SERVO_RIGHT 1320//1900  //1300
#define SERVO_LEFT  1720//1250  //1700
#define PROPOTION0 2
#define PROPOTION1 3
#define PROPOTION2 6
#define DERIVATIVE 1
//#define _DEBUG
//#define _SEND
//#define _AUTOSTOP
//#define _REGULATER
//#define _CENTER
//#define _LEFT
//#define _RIGHT  

void Init_ADC(void);
void Init_Capture(void);
void GetImage(void );
unsigned char  scan(register unsigned char *p);
unsigned char  scan2(register unsigned char *p);
unsigned char  GetLine(void);
void  Init_servo(void);
void  Init_motor(void);
void Car_control(void);
void filter(void);
void sci_init(void);
void sci_send(unsigned char ch);
void send(unsigned char *p) ;
void Speed_init(void);
void Speed_Control(void);
void find_Cur(void);
unsigned char find_S(void);

 static unsigned char x,y,SkipNum;
 static  unsigned char ImagePing[SAMPLELINE][SAMPLENUM];
 static unsigned char ImagePong[SAMPLELINE][SAMPLENUM];
 static  char ImagePar[SAMPLELINE];
 static char ImagePar2[SAMPLELINE];
 static char TimeCount=0;
 static char ImageGood[SAMPLELINE];
 unsigned char *ImageRDY;
 unsigned char PingOrPong;
 unsigned char PingSampling;
 unsigned char PongSampling;
 unsigned char Sampling=1;
 unsigned int  Sample_speed=0;
 char  Set_speed=0;
 unsigned int servoScale=SERVO_CENTER;
 unsigned int Last_servoScale=SERVO_CENTER;
 unsigned char Porpotion;
 unsigned char Sample_time=0;
 unsigned char Image_good=0;
 signed char top=0;
 unsigned char Front=0;
 unsigned char ON_THE_LINE=0;
 unsigned char ON_THE_S=0;
 
 static unsigned char const Speed_Tab[23]={
 23, //0
 22, //1
 10, //2
 10, //3
 10, //4
 10, //5
 20, //6
 19,//7
 18,//8
 18,//9
 18,//10
 17,//11
 17,//12
 17, //13
 17,//14
 15, //15
 10, //16
 5,  //17
 10,  //18
 10,   //19
 10,   //20
 10,   //21
 10  // 22
 } ;
 /*
 static unsigned char const P_Tab[12]= {
 5,//0
 5,//1
 5,//2
 5,//3
 5,//4
 6,//5
 6,//6
 6,//7
 6,//8
 6,//9
 5,//10
 5//11 
 }  ;
   */
 int Speed=0;
 int err_speed=0;
 int err=0;
 int LastErr=0;
 int errTop=0;	   
 int cur=0;   //curvature
 #ifdef _AUTOSTOP
  unsigned char errCount=0;
 #endif 
 
#pragma CODE_SEG __NEAR_SEG  NON_BANKED
/*******************************************/
interrupt void GetImageIsr(void) {  //CAPTURE ISR、场中断
     
     
     TCTL4_EDG0B^=1;	 //场同步捕捉信号翻转
     TCTL4_EDG0A^=1;
     
     TIE_C1I^=1;
    // PORTB_BIT0^=1;   //PING PONG INDICATOR  PORTB_BIT0位每采集一场数据翻转一下，这信号拔掉BDM后用示波器观察正常
    
     
    if (Sampling==1) {
     Sample_speed=PACN32;
     PACN32=0;
      x=0;y=0; SkipNum=0;Sampling=0; TFLG1_C1F=1;		//START ONE FRAME 
    } else {
      Sampling=1;
      if (PingOrPong==PING) {
        
          PingOrPong=PONG;		 //END OF ONE FRAME 
          PingSampling=0;
          PongSampling=1; 
         }else{
          PingOrPong=PING;
          PingSampling=1;
          PongSampling=0; 
         
         }
          #ifdef _DEBUG
          TIE_C1I=0;
          TIE_C0I=0;
         #endif 
        TFLG1_C1F=1;
        

    }
    TFLG1_C0F=1;			 //clear场同步interrupt flag 
    
}
/*******************************************/
interrupt void ReadLine(void) {    //IRQ 行中断
 register unsigned char i;
 register unsigned char *p;
 
 SkipNum++  ;
 if(SkipNum==20) {
  

 ATD0CTL5=0XA0;
 //PORTB_BIT1=1;   //AD开始采集数据时输出一个高电平
 if (PingOrPong==PING)
   p=&ImagePing[y][0];
 else
    p=&ImagePong[y][0];
 for (i=0;i<SAMPLENUM;i++) {
     while(ATD0STAT0_SCF==0);
      *p++= ATD0DR0L;
 }
 //PORTB_BIT1=0; //采集完后输出一个低电平，这些信号在拔掉BDM后也能正常出来的
 ATD0CTL5=0X00;
 SkipNum=0;

 y++;
 }
  TFLG1_C1F=1;//清除行中断标志
}

#pragma CODE_SEG DEFAULT


/*******************************************/
void main(void) {
  /* put your own code here */
  #ifdef _DEBUG
    unsigned char i;
  #endif 
  asm_main();
  DDRB=0XFF;
  Delay1ms(1000);
  DDRB=0X00;
  Delay1ms(1000);
  DDRB=0XFF;
  Delay1ms(1000); 
  DDRB=0Xff;
  PORTB=0XFF;
  INTCR_IRQEN=0; //DISABLE IRQ  
  TIE_C0I=0; //DISABLE CAPTURE INTERRUPT
 // LCD_LcmInit();
  Speed_init();
  Init_servo();  
  Init_motor();
  #ifdef _DEBUG
    sci_init();
  #endif 
 
  EnableInterrupts;
  
  #ifndef _DEBUG
  GetImage(); 
  for(;;) {}
  #else
    for(;;){
     TFLG1_C0F=1;
      TFLG1_C1F=1;
        
     GetImage();
     		 for (i=0;i<SAMPLELINE;i++)
 			    ImagePing[i][ImagePar[i]]=0;
 			#ifdef _SEND   
      send(&ImagePing[0][0]);
      #endif 
    }
  #endif 
  /* wait forever */
  /* please make sure that you never leave this function */
}
/*******************************************/
void Init_ADC(void) {
   
  ATD0CTL2=0XC0;  //POWER UP ADC ,DISABLE INTERRUPT 
  asm("nop");      //warm up 
  asm("nop");
  asm("nop");
  asm("nop");
  asm("nop");
  asm("nop");
  ATD0CTL3=0X08; 
  ATD0CTL4=0X80; 
  ATD0DIEN=0X00;
}
/*******************************************/
void Init_Capture(void) {
 TIOS_IOS0=0;  //INPUT CAPTURE ,VS INPUT
 TIOS_IOS1=0;  //INPUT CAPTURE ,HS INPUT
 TSCR1_TEN=1; //ENABLE TIMER 

 TCTL4_EDG0A=1; //  capture rising edge first
 TCTL4_EDG0B=0;
 
 TCTL4_EDG1A=1;
 TCTL4_EDG1B=0;
 
  x=0;y=0;
  TIE_C0I=1;  //ENABLE  CAPTURE INTERRUPT 
}
/*******************************************/
void GetImage(void) {

PingOrPong=PING; //SAMPLING PING FIRST 
Sampling=1;
PingSampling=1;
PongSampling=0;
Init_ADC();
Init_Capture();   //START SAMPLING 
#ifndef _DEBUG
for(;;) 
#endif 
{

 if (PingOrPong==PING) {
    // PORTB_BIT3=1; 
  while(PingSampling==1);
// control code
  //PORTB_BIT4=1;    
 	ImageRDY=&ImagePing[0][0];

 }
 else
 {
   // PORTB_BIT3=0;  
 
  while(PongSampling==1);
  //control code 								 //等待一场信息采集完
   //PORTB_BIT4=1;
   ImageRDY=&ImagePong[0][0];
 }
    Car_control();//控制舵机
    Speed_Control();
   // PORTB_BIT4=0;
}
  
}          
/******************************************/
  
unsigned char   GetLine(void) {
  unsigned char *p;
  volatile char i=0;
  unsigned int tc=0;
  char d=0;
  i=0;
  p=ImageRDY+VIDEO_LEFT;
  for (i=0;i<SAMPLELINE;i++) {
  *p=75;
  p+=SAMPLENUM;
  }
   p=ImageRDY+VIDEO_RIGHT;
  for (i=0;i<SAMPLELINE;i++) {
  *p=63;
  p+=SAMPLENUM;
  }
  p=ImageRDY;
  for (i=0;i<SAMPLELINE;i++) {
    ImagePar[i]=scan(p);								//比较从右向左和从左向右扫描获得的黑线位置的差异，如果大于2则认为此行数据有误，否则的话则认为没有
    ImagePar2[i]=scan2(p);
    if ((ImagePar2[i]>ImagePar[i]+2||ImagePar[i]>ImagePar2[i]+2)||ImagePar[i]==0) {
   
      ImagePar[i]=0;
      ImageGood[i]=0;
    } else {
      ImageGood[i]=1;
    }
       
    
    p=p+SAMPLENUM;
   }
 
 filter();
 
  top=0;
  i=SAMPLELINE-1;
  
  while(!(ImagePar[i]>0&&ImagePar[i-1]>0)&&i>0) i--;
  
  if (i==0) {
    
    return 0 ;
  } 
 // top=i;

  top=SAMPLELINE-1;
   
  do  {  //还原底部
     i++ ;  
      if (ImagePar[i]==0)
      {
        ImagePar[i]=2*ImagePar[i-1]-ImagePar[i-2];
      
        if (ImagePar[i]<0) ImagePar[i]=0;
        if (ImagePar[i]>VIDEO_RIGHT) ImagePar[i]=VIDEO_RIGHT;
      } 
      //tc+=ImagePar[i++];
  }   while (i<SAMPLELINE); 
    
     i=SAMPLELINE-2;
   if (ImagePar[i]>0) {
        
      do{
      i--;
         if (ImagePar[i]==0)
          ImagePar[i]=2*ImagePar[i+1]-ImagePar[i+2];
        
       } while (i>0 &&ImagePar[i]>VIDEO_LEFT&&ImagePar[i]<VIDEO_RIGHT) ;
       top=i;
   }
   if (ImagePar[top]>VIDEO_RIGHT||ImagePar[top]<VIDEO_LEFT) 
    ImagePar[top++]=0;
 /*
  if (top>0&&top<SAMPLELINE-1)   //还原顶部
     do {
      ImagePar[top-1]=2*ImagePar[top]-ImagePar[top+1];
      top--;
     }  while(top>0&&ImagePar[top]>VIDEO_LEFT&&ImagePar[top]<VIDEO_RIGHT) ;