main.c

该代码是在CodeWarrior 针对HC9SDT512单片机编程

#include <hidef.h>      /* common defines and macros */
#include <mc9s12xdp512.h>     /* derivative information */
#include <math.h>
#pragma LINK_INFO DERIVATIVE "mc9s12xdp512"


/************************/
int xie1[5];
int xie2[4];
unsigned char Last_State=0; 
unsigned char Last_Last_State=0;  //现在的状态
unsigned char state;
unsigned char isjiansu=0;
/******state=0代表现在一直处在直道********/
/******state=1代表现在车处在直道，但是前方探测到了弯道！********/
/******state=2代表现在车处在弯道********/
/******state=3代表图像空白********/
/******state=4代表车子不是处在正中央，但是车道是直道********/

unsigned char count=0;//真实的脉冲数
unsigned char CountLast=0;//0.2s内采集到的脉冲数，在0.2s内是个恒定值
unsigned char count1=0;//用来计数
unsigned char D1=40;
/*----------------------------------------------------------------------------*\
串口数据发送相关变量
\*----------------------------------------------------------------------------*/
#define Foscclk   64000000UL
#define Fpllclk   32000000UL
#define SCI1BAUD  600//不能太大

/*--------------------------------------------------------*\
图像处理变量
\*----------------------------------------------------------------------------*/
int min_line[6]={0,0,0,0,0,0};
#define field_signal PTH_PTH5 // field signal is sent into PortH_bit5
#define LINE_MIN   0 // first effective pint in each row
#define LINE_MAX   19// number of points sampled in each row
#define ROW_MAX    7 // number of rows needed to be sampled in each
#define video_center (LINE_MIN+LINE_MAX)/2  //center of the picture                 //picture
//#define ROW_START  50 // begin to sample from line start
#define ROW_END    250 // end flag of sampling
#define INTERVAL   31 // interval between effective rows
#define VALVE      20 // valve to decide black track or white track
#define FIRST_FIVE  5
/*----------------------------------------------------------------------------*\
舵机控制变量
\*----------------------------------------------------------------------------*/
int TurnAngle;
#define left_limit  1100 //
#define center      1500 //
#define right_limit 1900 //
#define Kp1          5// 
#define Kp2          30// 
#define Kd           10// 微分参数
/*----------------------------------------------------------------------------*\
速度控制变量
\*----------------------------------------------------------------------------*/
//#define curve_flag PORTE_BIT2 // indicate straight line or not
#define speed        PWMDTY1 // speed of the car
#define Steer        PWMDTY23 // steer of the car
#define CURVE_MAX    24 // valve to decide straight track or not
#define hign_speed   150 // speed used on straight track
#define low_speed    100// speed used on the turn
                     

unsigned char account=0;
int conlumn=0;
unsigned char ADT0=0;
unsigned char data[350];
int num=0;
//int i=0;
//int m=0;
//int k=0;
unsigned char ylc[6];
unsigned char row=0;

unsigned char ROW;
unsigned char LINE;
unsigned char black_x[ROW_MAX] ; // 0ne-dimensional array
unsigned char curve ; // valve used to decide straight 
                      //or turn
unsigned char curve_flag;  // indicate straight line or not
unsigned int  X;   //current location of the black wire
unsigned int row_count ; // row counter
unsigned int effective_row; // number of effective row
unsigned int current_black_wire; // location of current black wire 
unsigned int sum=0; // sum of current black wire 
unsigned char AVERAGE=0; // average of current black wire 
unsigned int currentD=0; // 当前的D参数，来自当前比例部分的值 
unsigned int beforeD =0; // 先前的参数，来自上次比例部分的值 
unsigned char Last_AVERAGE=0; //上次的AVERAGE值
/*----------------------------------------------------------------------------*\
函数部分
\*----------------------------------------------------------------------------*/

void InitSCI(){
      SCI1BD = Foscclk/2/SCI1BAUD/ 16;
      //SCI1BDL=208;
      SCI1CR1=0; //正常8位,循环工作使能 
      SCI1CR2 = 0x2C;
}

void ATD_OPEN(){

ATD0CTL2_ASCIE=1;
}

void ATD_CLOSE(){
ATD0CTL2_ASCIE=0;
}

void WriteToSCI(unsigned char *text,int len){
      while (len--){
        while (!(SCI1SR1 & 0x80));
        SCI1DRL = *text++;
        
      } 
}



void delay(){
  int i;
  for( i=0;i<30000;i++){
     }
}

void delay1(){
  int i,j;
  for( i=0;i<10000;i++){
   for( j=0;j<25;j++){
   }
     }
}

void writeToSCI(unsigned char text){
        while (!SCI1SR1_TDRE); //我日，为什么这是什么错误？
        SCI1DRL =text;
        //delay1();
       
}


 

void InitPLL(){  //总线频率为16M
SYNR=1;
REFDV=0;
while(CRGFLG_LOCK!=1);//判断锁相环是否锁住
CLKSEL=0xC0;
}


//每行采样64个点,78行 
//现在采集60*10个点  总计600
void ATD_init(){				 //ATD initiate

ATD0CTL2=0xC0;//关闭中断
ATD0CTL3=0x08;
ATD0CTL4=0x80;
ATD0CTL5=0xA1;
ATD0DIEN=0x00;
}


void Init_H(){
PPSH=0x50;
PIEH=0x40;//使能6脚，4脚在场中断中打开
}


void H_CLOSE(){
  PIEH=0x00;//使能4,6脚
}

void H_OPEN(){
  PIEH=0x50;//使能4,6脚
}


#pragma CODE_SEG DEFAULT

void InitPWM(void){
  PWME    = 0x00;     //禁止PWM输出
  PWMCTL  = 0x20;    //通道01不级联、23级联 
  PWMPRCLK= 0x33;  //ClockA = ClockB = Bus/8 = 4MHz
  PWMCLK = 0b00001110;     //1时钟来源选SA;23-SB;
  PWMSCLA= 1;   //PWMSCLA = PWMSCLB = 50
  PWMSCLB= 2;   // ClockSA=2MHz , ClockSB = 1MHz
  PWMPOL = 0xff;  //PWM输出起始为高电平
  PWMCAE = 0x00;   //左对齐方式
  PWMPER1= 200; //f=SA/200=10k 
  PWMDTY1= 90;  // PWM1控制电机
  //PWMPER23=20000; // frequency=50Hz(T=20ms)， PWM23控制舵机
 // PWMDTY23=1500;  //1000-1ms,左转到底;1500-1.5ms，居中;
                  //2000-2ms，右转到底  
  PWMPER23=10000; // frequency=100Hz(T=10ms)， PWM23控制舵机
  PWMDTY23=1450;  //1100-ms,左转到底;1450-1.5ms，居中;
                  //1900-ms，右转到底             
  PWME=0X0a;    //启动PWM输出1,23  
}
 
 void InitCapture(void){
  TIOS=0X00; //全部输入捕捉
  TSCR1=0X80;
  TSCR2=0X06;//禁止定时器溢出
  TCTL4=0X02;
  TIE=0X01; //0 1 2 3 口使能
  TFLG1=0X01; //清0 1 2 3中断
  DLYCT=0X01; //延时
  
}
 void Init_all(){
  DisableInterrupts;
  DDRK=0XFF;
  DDRB=0X00;
  PORTK=0XFF; 
  InitSCI(); 
  ATD_init() ;
  InitPLL();//注意这边，改变了总线周期，串口的时许也相应的改变了
  Init_H();
  InitPWM();
  InitCapture();
  EnableInterrupts;
  
  
 }
 //**********求绝对值*************************************//
//********************************************************//
unsigned char abs_sub(unsigned char num1, unsigned char num2)
{ unsigned char difference;
if(num1>=num2){
difference=num1-num2;
}else{
difference=num2-num1;
}
return difference;
}
 
 //**********黑线提取 *************************************//
//********************************************************//
void get_black_wire(void) // used to extract black wire
{ 
  unsigned int i,j,k;
  unsigned int min=100;
  
  unsigned int black_x[6]={0,0,0,0,0,0};
  sum=0;
  effective_row=0;
  for(i=0;i<6;i++){
    min_line[i]=0;
  }
  for(i=0;i<6;i++){
      for(LINE=10;LINE<30;LINE++){
          if(data[i*50+LINE]<=min){
             if(data[i*50+LINE]<min){
                min= data[i*50+LINE];
                j=LINE;
                k=j;
             } 
             else {
                min= data[i*50+LINE];
                k=LINE;
            }
      }
     }
      j=(j+k)/2;
      if(min<85){
       effective_row++;
       min_line[i]=j-10; 
      }
  }
  /*****处理以外的点********/
  for(i=0;i<4;i++){
  if((min_line[i+1]-min_line[i])*(min_line[i+1]-min_line[i+2])>0&&min_line[i]&&min_line[i+2]) 
      min_line[i+1]=(min_line[i]+min_line[i+2])/2;
  
  if((min_line[i]==0)&&(min_line[i+2]==0)) min_line[i+1]=0;
  }
 
 
  for(i=0;i<6;i++){
  sum=sum+min_line[i];
  }
  if(effective_row>0) {
    //等于0为无效图像！
  AVERAGE=sum/effective_row;  //若无图像 AVERAGE为上一周期的值
  }
  
 }
 
 
 /*************智能判别赛道的状况****************/
 void analysis(){
 unsigned char i=0;
 int sum=0;
     if(effective_row==6){
             for(i=0;i<5;i++){
            xie1[i]=min_line[i+1]-min_line[i];
           }
          for(i=0;i<4;i++){
            xie1[i]=xie1[i+1]-xie1[i];
          }
           for(i=0;i<4;i++){
            sum+=xie2[i];
           }
           if(abs(sum)<2) {
            state=4;
           isjiansu=0;
           }
           }
  
       else if(effective_row<=1) state=3;
       else if(abs_sub(AVERAGE,video_center)<2) {
        state=0;
        isjiansu=0;
       }
       else {
            state=2; //如果是弯道
            isjiansu=0;
           }
      if(Last_State==0&&state!=0) {
        state=1;
        isjiansu=1;
      }
      

 }
 
 
//**********速度控制 *************************************//
//********************************************************//
void speed_control(void)
{

switch(state) {
  case 0:
     speed=100;
     break;
  case 1:
     speed=0;
     break;
  case 2: {
    if(CountLast<12) speed=100;
     else speed=80;
     break;
  }
  case 3:
    if(!isjiansu){
    if(CountLast<8) speed=100;
    else speed=80;
    } 
    else speed=0;
    break;
  case 4:
    speed=80;
    break;
}
}

//**********舵机控制 *************************************//
//********************************************************//
void steer_control(void)
{   
    if(state==3){
       if(TurnAngle>1500)
       TurnAngle=1900;
       else TurnAngle=1100;
    }
      
    if((state!=3)&&(Last_State==3)){
    if(effective_row>4) {
    D1= abs_sub(AVERAGE,video_center);
    if(D1<2) TurnAngle=1500+D1*(AVERAGE-video_center);
    else TurnAngle=1500+(40+D1)*(AVERAGE-video_center); 
       }
} 
    else if(state!=3) 
    {
    D1= abs_sub(AVERAGE,video_center);
    if(state==4) TurnAngle=1500+(20+D1)*(AVERAGE-video_center);
    else TurnAngle=1500+(50+D1)*(AVERAGE-video_center);
    }       
    if(TurnAngle>1900)    TurnAngle=1900;
    else if(TurnAngle<1100)  TurnAngle=1100;
    Steer=TurnAngle;
}






 void CCDCollection(){ //CCD采集，当数据超过4992个时将数据发送出去
  //writeToSCI(0x56);
  unsigned char i,m;
  DisableInterrupts;
  for(m=0;m<50;m++){
    while(ATD0STAT1_CCF0==0);
    data[num*50+m]=ATD0DR0L;//这边注意一下   
  } 
  
 
 if(num>=5) {//注意这边是77 
    count1++;
    if(count1>=4){ //每隔0.08s采集一次脉冲
      count1=0;
      CountLast=count; 
      count=0;
    }
      
     
 
 
if((PORTB&0X01)==0X01) 
   {
   get_black_wire();
   analysis();
   steer_control();
   speed_control(); 
   writeToSCI(255);
   //writeToSCI(CountLast);
   delay1();
   writeToSCI(AVERAGE);
   delay1();
   writeToSCI((Steer-1000)/10);
   delay1();
   for(i=0;i<6;i++){
     writeToSCI(min_line[i]);
     delay1();
   }
  //Last_AVERAGE=AVERAGE;

 } 
 
 else {
  get_black_wire();
  analysis();
  steer_control();
  speed_control();
  Last_AVERAGE=AVERAGE;
  Last_Last_State=Last_State;
  Last_State=state;
 }
     num=0;
     PIEH_PIEH4=0;
     PIEH_PIEH4=0;
   } 
   
   
    EnableInterrupts;
}

 #pragma CODE_SEG __NEAR_SEG NON_BANKED    
 interrupt 8 void  T_ISR(void){            //测速模块
    DisableInterrupts;
    count++;
    TFLG1=0X01;
    EnableInterrupts;
 }
 
 
 
 #pragma CODE_SEG DEFAULT
#pragma CODE_SEG __NEAR_SEG NON_BANKED 
interrupt 25 void ISR()
{	
	                               //场同步，行同步中断
 if(PIFH_PIFH6==1)  //这边改过
 { 
                                  //场同步中断执行程序
  num=0;                       
  PIFH_PIFH6=1;
  conlumn=0;
  PIEH_PIEH4=1;
  PIEH_PIEH4=1;
  
  
 } 

else{  
              //行同步中断执行程序,是后沿
    PIFH_PIFH4=1;
    PIFH_PIFH6=1;
    if(conlumn>=30&&conlumn%28==1&&num<6) {
     CCDCollection();
     num++;
     
    }
    conlumn++;
   
}  
}
 
void main(void) {
   unsigned char i; 
   Init_all();
   if((PORTB&0X01)==0X01) 
   {
   for(i=0;i<20;i++){
   writeToSCI(0x00);
   }
   } 
   for(;;) {

 }  
}