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 5000 //舵机中心位置       4900
#define SERVO_R 1100     //右极限            1100
#define SERVO_L -1100    //左极限             -1100

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

//系统参数类
unsigned char AutoFlag=1;     //手自动循线标志 1:自动 0:手动
unsigned char DebugEn=1;      //调试信息使能
unsigned char CaptureCommand; //串口命令
unsigned char BadFrameCnt;    //坏帧计数
unsigned char ErrLineCnt;     //错误行计数
//图像数据
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;  //定义设定车速及最大、最小车速
int Speed_z,Speed_w,Speed_bad,Speed_setlast;
int slow_en,middle_en,fast_en;
int Bad_jia,Bad_jian;
//*********************
//    函数声明部分
//*********************

//  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);		//启动延时
  /*系统IO初始化*/
  init_PA();
  init_PB();
  init_PAD();
  init_IO();
  init_PWM();
  init_ECT();
  sci_init();
  init_IIC();
  
  /*外围设备初始化*/
  init_CMOS();
  //根据拨码开关确定图像曝光设置，适应不同光照环境
  //若拨码开关第二位为1，使用手动曝光设置
  //若为0，则使用自动曝光 适合光线充足情况
  if(PTP_PTP1==1){
    iic_write(0x13,0x00);
    iic_write(0x10,0x65);     //理论计算结果为0x46
  }
  else{
    iic_write(0x13,0x01);
  }
                             
  
  DRV1=PTP_PTP0;      //是否打开电机
  DRV2=1;
  EnableInterrupts;
  init_Control();
  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();
      //LED=!LED;
    }
    //TSCR1_TEN=PTT_PTT6; //根据DIP第7位状态确定是否开启监控输出a b Ee2
    
    }
  }    


//*********************
//    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;
  }
}

/*
void read_all(void){
  int read_i,read_j;
  int cnt=0;
  read_i=0;
  read_j=0;

  while(EF){
    R_en=0;
    VideoData[read_i][read_j]=PORTA;
    Histogram[VideoData[read_i][read_j]]++;//直方图建立
    cnt++;
    R_en=1;
    read_j++;
    if(read_j==SIZE_X){
      read_i++;
      read_j=0;
    }
  }
  read_j=SIZE_X;
}
*/

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

#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语句结束*/ 
}