main.c

武汉大学参加飞思卡尔智能汽车大赛的小车程序

#include <hidef.h>      /* common defines and macros */
#include <string.h>
#include <mc9s12dg128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"

/*PortB各管脚名称定义*/
#define VSYN      PORTB_BIT0
#define DRV1			PORTB_BIT1
#define DRV2      PORTB_BIT2
#define V_en      PORTB_BIT3
#define LED_en    PORTB_BIT4
#define EF        PORTB_BIT5
#define R_en      PORTB_BIT6
#define FifoReset PORTB_BIT7
#define LED       PTP_PTP7

/*系统常量定义*/
#define SIZE_X 60       //定义列数
#define SIZE_Y 12		  	//定义行数
#define DATASIZE 720    //图像总点数
#define SERVO_MID 4380  //舵机中心位置       4400
#define SERVO_R 1250     //右极限            1230
#define SERVO_L -1250    //左极限             -1250



//*********************
//    全局变量定义
//*********************

//系统参数类
unsigned char AutoFlag=1;     //手自动循线标志 1:自动 0:手动
unsigned char DebugEn=1;      //调试信息使能
unsigned char CaptureCommand; //串口命令
unsigned char BadFrameCnt;    //坏帧计数
unsigned char ErrLineCnt;     //错误行计数
unsigned int  time=0;           //系统计时

//图像数据
unsigned char VideoData[SIZE_Y][SIZE_X]; //图像信息存储空间
unsigned char Threshold=60;       //两像素之间灰度差阈值 80

//控制参数
signed char diff[SIZE_Y];       //每行黑线与中心的偏差
unsigned char BadFrame=0;       //坏帧标志位

//最小二乘法拟合参数
signed int a,b;
unsigned long Ee2;

//小车转角控制参数

//单行转角控制参数
unsigned char line;		                //单行控制采集的线
signed char diff_sum,diff_last;       //偏差之和、上次偏差
signed char P1,I1,D1;                 //PID参数
unsigned char P1_en,I1_en,D1_en;      //PID参数使能
signed int P1_out,PID1_out;            //PID输出

//斜率控制参数
unsigned char Pa_en,Da_en;            //PID参数使能
signed char a_last;                   //上次斜率偏差
signed char Pa,Da;                    //PID参数
signed int  Pa_out,PIDa_out;          //Pa输出

signed int PID_out;                   //PID总输出
signed int PID_out1,PID_out2,PID_out3;//前N次的PID输出
signed int servo_out;                 //舵机输出偏差量

/*速度控制部分*/
unsigned int Speed_cnt,Speed_set;  //测速脉冲个数,设定车速
signed int Speed_d=0,Speed_d_last=0;  //定义设定车速及最大、最小车速

/*拨码开关速度设定*/
unsigned int Speed_zhi,Speed_s,Speed_wan,Speed_bad;

//*********************
//    函数声明部分
//*********************

//  main.c
void delay_us(unsigned int time);
void delay_ms(unsigned int time);
void reset_FIFO(void);
void read_all(void);
void send_data(void);
void send_num(signed int);

//  init.c
void init_PLL(void);
void init_CMOS(void);
void init_PWM(void);
void init_ECT(void);
void init_PB(void);
void init_PA(void);
void init_PAD(void);
void init_IO(void);

//  IIC.c
void init_IIC(void);            //init DG128's iic port
void iic_write(byte sub_address,byte write_record); //write byte to iic client

//  printp.c
void sci_init(void);            //init DG128's sci port
void sci_putchar(char data);    //send a char
void sci_prints(char* ctrl);    //send a string

//  Control.c
void ScanLine(void);
void CalcuLine(void);
void init_Control(void);
void ServoControl(void);
void SpeedControl(void);



//*********************
//      主程序
//*********************
void main(void){
  init_PLL();         //系统时钟设定：24MHz
  delay_ms(1000);		//启动延时
  /*系统初始化*/
  init_PA();
  init_PB();
  init_PAD();
  init_IO();
  init_PWM();
  init_ECT();
  sci_init();
  init_IIC();
  init_Control();
  init_CMOS();
  
  
  //曝光设置
  //根据拨码开关第2位确定图像曝光设置，适应不同光照环境
  //  拨码开关第2位   曝光设定
  //      0           自动曝光（光线充足）
  //      1           手动曝光（光线较暗）
  if(PTM_PTM1==1){
    iic_write(0x13,0x00);
    iic_write(0x10,0x60);     //理论计算结果为0x46
  }
  else{
    iic_write(0x13,0x01);
  }
  
  //电机设置                           
  //拨码开关第一位决定是否开机自动打开电机
  //  拨码开关第1位   电机状态
  //      0           开
  //      1           关
  DRV1=PTM_PTM0;    //是否打开电机
  DRV2=1;

  
  
  //速度设定
  /*Speed_zhi=150;
  Speed_wan=110;
  Speed_bad=90;
  Speed_s=155;
  */
  
  
  //直道速度--拨码开关1、2、3号
  if(PTP_PTP0==0&&PTP_PTP1==0&&PTT_PTT2==0)     Speed_zhi=110;			 
  else if(PTP_PTP0==0&&PTP_PTP1==0&&PTT_PTT2==1)Speed_zhi=120;			 
  else if(PTP_PTP0==0&&PTP_PTP1==1&&PTT_PTT2==0)Speed_zhi=130;			
  else if(PTP_PTP0==0&&PTP_PTP1==1&&PTT_PTT2==1)Speed_zhi=140;		
  else if(PTP_PTP0==1&&PTP_PTP1==0&&PTT_PTT2==0)Speed_zhi=150;		
  else if(PTP_PTP0==1&&PTP_PTP1==0&&PTT_PTT2==1)Speed_zhi=160;		
  else if(PTP_PTP0==1&&PTP_PTP1==1&&PTT_PTT2==0)Speed_zhi=170;		
  else                                          Speed_zhi=180;
  
  //弯道速度--拨码开关4、5、6号
  if(PTT_PTT3==0&&PTT_PTT4==0&&PTT_PTT5==0)     Speed_wan=80; 
  else if(PTT_PTT3==0&&PTT_PTT4==0&&PTT_PTT5==1)Speed_wan=90;  
  else if(PTT_PTT3==0&&PTT_PTT4==1&&PTT_PTT5==0)Speed_wan=100;
  else if(PTT_PTT3==0&&PTT_PTT4==1&&PTT_PTT5==1)Speed_wan=110;
  else if(PTT_PTT3==1&&PTT_PTT4==0&&PTT_PTT5==0)Speed_wan=120;
  else if(PTT_PTT3==1&&PTT_PTT4==0&&PTT_PTT5==1)Speed_wan=130;
  else if(PTT_PTT3==1&&PTT_PTT4==1&&PTT_PTT5==0)Speed_wan=140;
  else                                          Speed_wan=150;
  
  //BadFrame速度--拨码开关7、8号
  //1:高速  0:低速
  if(PTT_PTT6==0&&PTT_PTT7==0)      Speed_bad=85; 		 
  else if(PTT_PTT6==0&&PTT_PTT7==1) Speed_bad=90;				
  else if(PTT_PTT6==1&&PTT_PTT7==0) Speed_bad=95;
  else                              Speed_bad=100;
  
  if(Speed_zhi>=170){
    Speed_s=Speed_zhi;
  }
  else{
    Speed_s=Speed_zhi+5;
  }  
  
    
  //TSCR1_TEN=1;
  EnableInterrupts;
  for(;;){
    if(AutoFlag==1){
      /*图像数据写入FIFO*/
      reset_FIFO();
      PACN0=0;
      ICPAR_PA0EN=1;             
      while(VSYN!=0);
      while(VSYN!=1);
      V_en=1;
      while(PACN0<SIZE_Y);
      V_en=0;
      ICPAR_PA0EN=0;
      /*图像数据写入FIFO完成*/
      
      /*速度控制,若能保证每次控制周期在16ms内完成，则
      每次得到的计数脉冲应为16.67ms内的脉冲数，计速可靠*/
      SpeedControl();
      /*读图像数据至RAM*/
      read_all();

      /*控制算法部分*/
      ScanLine();
      CalcuLine();
      ServoControl();
    }
    time++;
    
    /*
    //正常冲刺方案
    if(PTS_PTS2){
      //开启第二圈冲刺方案
      //初赛14s后，决赛25s后，设定速度++
      if((time==840&&PTS_PTS3==0)||(time==1500&&PTS_PTS3==1)){
        Speed_zhi+=10;
        Speed_wan+=5;
        Speed_bad+=2;
        Speed_s+=5;
      }
    }
    */
    
    //疯狂版方案
    //时间一到，立刻自爆
    if((time==840&&PTS_PTS2==0&&PTS_PTS3==1)||(time==1500&&PTS_PTS2==1&&PTS_PTS3==0)||(time==1800&&PTS_PTS2==1&&PTS_PTS3==1)){
      Speed_bad+=30;
    }
    
    
    }
  }




//*********************
//    main.c函数
//*********************

//FIFO复位
void reset_FIFO(void){
  FifoReset=0;
  asm{
    nop;
    nop;
    nop;
    nop;
    nop;
    nop;
    nop;
  } /*延迟至少150ns*/
  FifoReset=1;
}
              

void read_all(void){
  int read_cnt;                                  
  byte *VideoData_ptr;
    VideoData_ptr=VideoData;
   
  for(read_cnt=0;read_cnt<DATASIZE;read_cnt++){
    R_en=0;
    *(VideoData_ptr+read_cnt)=PORTA;
    R_en=1;
  }
}

//******************
//控制及图像处理部分
//******************

#include "Control.c"

//******************
//系统功能函数
//包括延迟和串口通讯
//******************

/*延迟函数delay_us(),delay_ms()*/
//微秒级延时函数
void delay_us(unsigned int time){
  for(;time>0;time--){
    asm{
      nop;
      nop;
      nop;
      nop;
      nop;
      nop;
      nop;
      nop;
      nop;
      nop;
    }
  }
}

//毫秒级延时函数  
void delay_ms(unsigned int time){
  static int delay_i;
  for(;time>0;time--){
    for(delay_i=0;delay_i<2517;delay_i++){
      asm{
        nop;
        nop;
        nop;
      }
    }
  }
} /*---延迟函数---*/


//串口发送一帧图像数据至串口
void send_data(void){
  int send_cnt;
  byte *data_ptr;
  data_ptr=VideoData;
  
  for(send_cnt=0;send_cnt<DATASIZE;send_cnt++){
    sci_putchar(*(data_ptr+send_cnt));
  }
}

//发送一个有符号的三位数至串口
void send_num(signed int num){
  if(num<0){
		  num=-num;
		  sci_putchar('-');
		}
		sci_putchar(num/100+48);
    sci_putchar(num%100/10+48);
    sci_putchar(num%10+48);
    sci_putchar(' ');
}


//****************
//系统中断处理部分
//****************
#pragma  CODE_SEG  NON_BANKED

void interrupt 10 DebugSend(void){
  unsigned char i;
  TFLG1_C2F=1;	          //clear the interrupt flag
  
  sci_prints("d=");
  send_num(diff[line]);
  sci_prints(" a=");
  send_num(a);
  sci_prints(" b=");
  send_num(b);  
  sci_prints(" E=");
  send_num(Ee2/10);
  sci_putchar('\n');
  sci_putchar('\r');

}


//SCI输入中断
void interrupt 20 RxInterrupt(void){
	 //byte i;
	 DisableInterrupts;
   //i=SCI0SR1_RDRF;
   if(SCI0SR1&0x20){
    CaptureCommand=SCI0DRL;
   }

   
   switch(CaptureCommand){
    case '1':
    case '7':
      reset_FIFO();
      PACN0=0;
      ICPAR_PA0EN=1;
      while(VSYN!=0);
      while(VSYN!=1);
      V_en=1;
      while(PACN0<SIZE_Y);
      //while(VSYN!=1);
      V_en=0;
      ICPAR_PA0EN=0;
      read_all();
      ScanLine();
      send_data();
      break;
      
    case '2':
      DRV1=!DRV1;
 			break;		
    case '3':							//Left
      PWMDTY45=PWMDTY45-100;
      break;
    case '4':						  //Center
  		PWMDTY45=SERVO_MID;
  		break;
    case '5':							//Right
      PWMDTY45=PWMDTY45+100;
      break;
    case '6':             
      ServoControl();
			break;
    case '8':             //加速
      PWMDTY3+=8;
      break;
    case '9':             //减速
      PWMDTY3-=8;
      break;
    case '0':
      AutoFlag=!AutoFlag; //手自动切换
      break;
    case 'd':
    case 'D':
      DebugEn=!DebugEn;
      break;
    case 'b':
    case 'B':
      if(PWMPOL==0x20){
        PWMPOL=0x2C;
      }
      else if(PWMPOL==0x2C){
        PWMPOL=0x20;
      }
      break;
		default:
		  break;
   }
   EnableInterrupts;
   /*switch语句结束*/ 
}