freescale-smartcar.txt

飞思卡尔杯汽车邀请赛北理傲雄车队技术报告

附录A  模型车控制主程序代码
#include <hidef.h>      /* common defines and macros */
#include <mc9s12dp512.h>     /* derivative information */
#include "STDINC.h"
#include "stdlib.h"
#include "sci.h"
#pragma LINK_INFO DERIVATIVE "mc9s12dp512"
//////////////////////////参数设定//////////////////////
//舵机直行的PWM--2400
//马达60%--1200
#define OK 1
#define WRONG 0
const int16 RUDKP=33;//50;
const int16 RUDKD=5;//5;
const float MOTKP=0.005263;
const float MOTKI=0.005263;
const float MOTKD=0.005263;

const float CONSTANT1=136;
const float CONSTANT2=5;
const float CONSTANT3=4;
const float CONSTANT4=3;
int16 RErrLast=0;
int16 RErrCurr=0;
int16 RErrPreLast=0;
int16 Ruddelta=0;
int16 RudPWM;
int16 SenFlg=0;

int16 SpeedCurr=0;
int16 SpeedLast=0;
int16 Speed=0;
double SpeedFreq=0; 

int16 Motdelta=0;
int16 MotPWM=1000;
int16 SErrLast=0;
int16 SErrCurr=0;
int16 SErrPreLast=0;
int16 MotDD=0;
/////////////////速度参数///////////////////////////
 float Pdelta[4]={0.94,0.82,0.52,0.65};
 float Ddelta[4]={0.4,0.2,0.21,0};
int16 Vline=3040;
int16 Vscor=2565;
int16 Vlcor=2280;
int16 Vbrek=2375;
int16 Vsbrk=1330;
int16 Vstri=2600;
//Global variables
int16 SenCurBacW,SenLasBacW;
int16 TarSpeed,LastTarSpeed;
float Modulus,TarSpeedF;
int16 TarFlg=0;
uint8 sensorA=0,sensorB=0;
uint8 SensorA=0,SensorB=0;   
int16 Sensor;      
uint16 CounterCurr,CounterLast,Counter;
int16 tmpflg;
int16 look;
int16 LapFlg1=0;
int16 LapFlg2=0;
int16 Flag=0;
int16 temp=0;
int16 LapNum=0;
int16 LNum=0;
int16 FLAG=0;
int16 LFlg=0;
int16 delay1=0;
int16 StopFlg=0,StopNum=0;

int16 j=0;
int16 tmp[39]={0};
int16 num=39;
int16 s1,s2,s3;
int16 Tar,cnt1,cnt2,cnt3,cnt4,cnt5;                
int16 flag4;


//////////////////////////函数声明//////////////////////
void atd_init(void);
void motor_ctrl(void);
void readsensor(void);
void target_speed(void);
void readspeed(void);
void rudder_ctrl(void);
int8 path_recog(void);
void sort(void);
void startline(void)
////////////////////////中断函数/////////////////
#pragma CODE_SEG __NEAR_SEG NON_BANKED
interrupt void readsensor(void){
 DisableInterrupts;
 sensorA=PTM;           
 sensorB=PORTB;            
 
 sensorA=sensorA&0x3F;   //clean unwanted bit
 sensorB=sensorB&0x3F;    
 
 Flag=0; 

 if(LFlg==1)
 {
 delay1++;
 if(delay1>=5) 
 {
 LFlg=0;
 delay1=0;
 }
 }
 else startline();
 
 if(StopFlg==1)
 {
 StopNum++;
 }

 TarFlg++;
 if(LapNum>2) {
 PWMDTY45=1000;
 }
 PWMDTY01=1500;
 MCCNT=20000;         
 MCFLG_MCZF=1;
 EnableInterrupts;
}
uint16 plsnum=0;
#pragma CODE_SEG __NEAR_SEG NON_BANKED
 interrupt void readspeed(void){
  TFLG1_C3F=1;
  //CounterLast=CounterCurr;
  CounterCurr=TC3;
  CounterLast=TC3H;
  switch(TFLG2_TOF) 
  {
    case 0:Counter=CounterCurr-CounterLast;
           break;
    case 1:Counter=65535-CounterLast+CounterCurr;
           TFLG2_TOF=1;
           break;
  }
  if(Counter<16)
  {                                          
     Counter=16;
  }
  Speed = 1000000/Counter;//190;//((float)Speed*3/4+(float)SpeedFreq/4);         
  
 if(LapNum==1)
 {
  plsnum++;
 }
  if(LapNum>1) 
 {
  StopFlg=1;
  if(StopNum>100) 
  {
    PWMDTY45=1000;
  }
 }
 else 
 motor_ctrl();
 //FLAG++;
 }                                                       
 
////////////////////////函数定义//////////////////////////////////

void pwm_init(void){
 PWMPOL = 0xFF;   //polarity,high at the beginning;
 PWMCLK = 0x2A;   //select SA/SB as the pwmclk;SA clock for STEER;SB clock for MOTOR;
 PWMPRCLK = 0x11; //A=bus clock/2=16MHz,B=bus clock/2=16MHz;
 PWMCAE = 0x00;   //PWM flush left
 PWMSCLA = 0x01;  //SA=A/2;PWM clock=bus clock/2/2/1=8MHz
 PWMSCLB = 0x05;  //SB=B/1;PWM clock=bus clock/2/2/5=1.6MHz
 PWMCTL = 0xF0;   //concatenate 0&1,2&3;

 PWMPER45 = 2000;  //output frequency=SA clock/2000=4KHz;for MOTOR using
 PWMDTY45 = 1000;  //PWM percent is 65%; 

 PWMPER23 = 16000;    //output frequency=SB clock/16000=100Hz;for RUDDER using
 PWMDTY23 = 2400;     //PWM percent is 15%  down--LEFT
 
 PWME=0x2A;
}
void ect_init(void) {

 MCCTL=0xC2;         //MCZI MODMC RDMCL ICLAT FLMC MCEN MCPR1 MCPR0=11000010B  clock=Fbus/8
 MCCNT=20000;         //5ms every interrupt
 MCFLG=0x80;         //set underflow flag
 //////////////////////////////PACA_init//////////////////////////////////////
  TIOS=0X00;             //input capture selection,channal 6: output compare channal;
  TCTL3=0X01;
  TCTL4=0X40;                 //capture on the rasing edge;
  TIE=0X18;                     //enable interrupt when captured;       
  TSCR2=0X05;                //bit7:overflow interrupt disable,bit[2:0]:timer=bus clock/32=1MHz
  ICOVW=0X00;               //overwrite by the new data;
  ICSYS=0X02;              //queue mode;
  TSCR1=0x80;              //enable the timer;
  MCCTL_MCEN=1; 
}
//////////////////////////////sci_init/////////////////////////////////
void SciInt(void){
   
  SCI0BDL=0xD0; //总线为32M，波特率为9600
  SCI0CR2=0X2C; //允许中断，允许发送，允许接受
       
   }
   
void SCI_Trans(byte data) {
  while (!SCI0SR1_TDRE);//SC0DR处于忙状态，等待。
  SCI0DRL=data;
}
void systemboot(void) {
 SYNR = 7;      //Fbus=Fosc*(SYNR+1)/(REFDV+1)=32MHz
 REFDV = 3;
 PLLCTL = 0x71;
 while(CRGFLG_LOCK==0){}
 CLKSEL = 0x80;

 DDRA = 0xFF;     //PortA is input
 PORTA=0x00;        //clean PortA
 DDRB = 0x00;     //PortB is input
 PORTB=0x00;        //clean PortB
 PORTK= 0x00;
 DDRK = 0x00;
 PTM  = 0x00;
 DDRM = 0x00;
 pwm_init();
 ect_init();
 SciInt();
 //////////////////////////////////////////
 if(PORTK_BIT0==1&&PORTK_BIT1==0)
 {
  Vline=2755;
  Vscor=2375;
  Vlcor=2280;
  Vbrek=2185;     //可尝试变更2090-2185
  Vsbrk=1330;     
  Vstri=2400;
  Pdelta[0]=0.72;
  Pdelta[1]=0.77;
  Pdelta[2]=0.83;
  Ddelta[0]=0.4;
  Ddelta[1]=0.2;
  Ddelta[2]=0.21;
 } 
 else if(PORTK_BIT0==0&&PORTK_BIT1==1)
 {
  Vline=3135;
  Vscor=2660;
  Vlcor=2375;
  Vbrek=2440;     //可尝试变更2090-2185
  Vsbrk=1330;     
  Vstri=2700;
  Pdelta[0]=0.67;
  Pdelta[1]=0.72;
  Pdelta[2]=0.77;
  Ddelta[0]=0.4;                            
  Ddelta[1]=0.2;
  Ddelta[2]=0.21;
 } 
 else if(PORTK_BIT0==1&&PORTK_BIT1==1)
 {
  Vline=2565;
  Vscor=2280;
  Vlcor=2185;
  Vbrek=2185;     //可尝试变更2090-2185
  Vsbrk=1330;     
  Vstri=2300;
  Pdelta[0]=0.67;
  Pdelta[1]=0.72;
  Pdelta[2]=0.77;
  Ddelta[0]=0.4;
  Ddelta[1]=0.2;
  Ddelta[2]=0.21;
 }
 if(PORTK_BIT2==1){Vscor=Vscor+95;Vstri=Vstri+95;}
 if(PORTK_BIT3==1){Vscor=Vscor+190;Vstri=Vstri+190;}
 if(PORTK_BIT4==1){Vlcor=Vlcor+95;}
 if(PORTK_BIT5==1){Vlcor=Vlcor+190;}
 if(PORTK_BIT7==1){Vline=Vline+95;}
 if(PORTB_BIT7==1){Vline=Vline+190;}
}
#pragma CODE_SEG DEFAULT

void main(void) {
  /* put your own code here */
  DisableInterrupts;
  systemboot();
  asm{ 
      nop;
     }
  EnableInterrupts;

  for(;;) {
     
  path_recog();
  sort();
  rudder_ctrl();

  } /* wait forever */
  /* please make sure that you never leave this function */
}
/////////////////MAIN END//////////////
/////////////////PATH RECOGNISE////////
int8 path_recog(void){
  int16 i;
  int16 sen_num=-6;  
  int16 sen_cnt=0;    
  int16 sen_pos[12];   
  uint8 mask;
  tmpflg=0;
  
  mask=0x20;		
  for(i=-10;i<=0;i+=2) 
  {
    if((mask&SensorA)==0) 
    {
       tmpflg+=i;
       sen_pos[sen_cnt]=sen_num;
       sen_cnt++;  
    }
    sen_num++;
    mask>>1;
  }
  
  mask=0x01;		
  for(i=0;i<=10;i+=2) 
  {
    if((mask&SensorB)==0) 
    {
       tmpflg+=i;//tmpflg=tmpflg+i-1;
       sen_pos[sen_cnt]=sen_num;
       sen_cnt++;  
    }
    sen_num++;
    mask<<1;
}
  if(sen_cnt==0) 
  {
    if(Sensor>=8)                 
		{	 	    
			Sensor=10; PWMDTY23=2160;
			if(MotPWM<=1370) PWMDTY01=1370;
			else PWMDTY01=MotPWM-30; 
			Flag=1;
		} 
		else if(Sensor<=-8)
		{
			Sensor=-10; PWMDTY23=2640;
			if(MotPWM<=1370) PWMDTY01=1370;
			else PWMDTY01=MotPWM-30; 
			Flag=1;
		} return 0;
  }
   if(sen_cnt==1);
  else for(i=1;i<sen_cnt;i++)          
  {
   if(sen_pos[i]-sen_pos[i-1]>=2) 
   {
    return 0;
   }
  }        
   look=tmpflg;
   Sensor=tmpflg/sen_cnt;
   return 0;
 }
 void startline(void){

  if(       ((sensorA & 0x30)==0x00||(sensorA & 0x18)==0x00)
         // ((sensorB & 0x30)==0x00||(sensorB & 0x18)==0x00)
         && ((sensorA & 0x03)==0x03||(sensorB & 0x03)==0x03)
         && ((sensorA & 0x06)!=0x00||(sensorB & 0x06)!=0x00)) 
         {
          //LapNum++;
          LapFlg1=1;
          FLAG=1;
          LNum=0;
         }
  if(    // ((sensorA & 0x30)==0x00||(sensorA & 0x18)==0x00)
            ((sensorB & 0x30)==0x00||(sensorB & 0x18)==0x00)
         && ((sensorA & 0x03)==0x03||(sensorB & 0x03)==0x03)
         && ((sensorA & 0x06)!=0x00||(sensorB & 0x06)!=0x00)) 
         {
          LapFlg2=1;
          FLAG=1;
          LNum=0;
         }
         
  if(LapFlg1==1&&FLAG==1)     
   {
    if(LapFlg2==1) 
    {
      LapNum++;
      LapFlg1=0;
      LapFlg2=0;
      FLAG=0;
      LNum=0;
      LFlg=1;
    }
   } 
if(LapFlg2==1&&FLAG==1)     
   {
    if(LapFlg1==1) 
    {
      LapNum++;
      LapFlg2=0;
      LapFlg1=0;
      FLAG=0;
      LNum=0;
    }
   }
 }
 ////////////////Delta Kp///////////////

 ////////////////RUDDER CONTRAL/////////
 void rudder_ctrl(void){
  float Kp,Kd;
  if(Sensor>=8||Sensor<=-8) temp=0;
  else if(Sensor>4||Sensor<-4) temp=1;
            else temp=2;
  RErrLast=RErrCurr;
  RErrCurr=Sensor;