freescalesmartcar.c

飞思卡尔智能小车程序,综和了前几届几个队伍的程序

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

#pragma LINK_INFO DERIVATIVE "mc9s12dg128b" 
 
                   
 
 
#define N   200 
#define ini_Angle    530 
#define VV_DEADLINE 1 
#define RoundNumber 1 
 
 
unsigned char AD_Data[15]; 
unsigned char MaxAd[15]; 
unsigned char MinAd[15]; 
unsigned char percent[15]; 
unsigned char max,min; 
int printcount=0; 
int frequent=0; 
 
int dif_distance=0; 
int dif1; 
int distance; 
int pre_distance=0; 
uchar nowposition; 
uchar preposition; 
uchar flag=0; 
uchar blackflag=0; 
int error1num=100; 
int error1[100]={0}; 
 
 
uchar 
speed[5][5]={{100,130,150,160,170},{90,120,140,150,160}, 
{80,100,120,130,145},{80,130,145,155,180},{100,130,145,155,165}};  
uchar speedlevel;                                            
int VV_MAX=1000,VV_MIN=0; 
 
int VV_KDVALUE =7 ; 
int VV_KPVALUE =15 ; 
int VV_KIVALUE =20; 
 
int isstart=0,startline=0,iscrossing=0; 
 
int distance1,pre_distance1; 
uchar max1,min1; 
 
 
int cout=0; 
 
 
void initiaPLL(void){ 
   REFDV=1; 
   SYNR=3; 
   while(0==CRGFLG_LOCK); 
   CLKSEL=0x80; 
}  
 
void delay(int n) { 
  int i,j; 
  for(i=0;i<n;i++) 
  for(j=0;j<0xffff;j++); 
} 
int abs(int num) 
{ 
  if(num<0) 
    return -num; 
  else 
    return num; 
} 
 
void GetADValue(void); 
 
 void initialSCI(void){ 
SCI0CR2=0x0c; 
SCI0BDH=0x00; 
SCI0BDL=0x0d0; //24M 
//SCI0BDL=0x34;  //8M 
} 
 
void intialPWM(void) { 
   
  PWME=0x00;           //关 PWM 
  PWMPRCLK=0x00;//预分频选择总线时钟 
  PWMSCLB=40;//120 分频     =0.2MHZ 
  PWMCLK_PCLK3=1;//选择 CLOCKSB 时钟 
  PWMPOL=0xff;//开始输出高电平 
  PWMCAE_CAE3=0;//左对齐 
  PWMCTL_CON23=1;//级连 
  PWMPER23=4000;//频率=CLOCKSB/(PWMPERx+1)      =100HZ 
  PWMDTY23=ini_Angle;//占空比=(PWMDTYx+1)/(PWMPERx+1) 
  PWME_PWME3=1;  
   
 
  PWMSCLA=16;//24 分频     =1MHZ 
 // PWMSCLA=240;//480分频  =50KHZ 
  PWMCLK_PCLK1=1;//选择 CLOCKSA时钟 
  PWMPOL=0xff;//开始输出高电平 
  PWMCAE_CAE1=0;//左对齐 
  PWMCTL_CON01=1;//级连 
  PWMPER01=1000;//频率=CLOCKSA/(PWMPERx+1)      =1kHZ 
  PWMDTY01=0;//占空比=(PWMDTYx+1)/(PWMPERx+1) 
  PWME_PWME1=1;   
} 
 
 
void initialECT(void){ 
   TSCR2_PR = 7;   //divide 128 
        TIOS = 0b00000110;    //1,2,,The corresponding channel acts as an output compare. 
    TCTL2 = 0b00010100;    //1,2,,Output Mode 
    TCTL4 = 0b00000010;     //0,Capture on falling edges only  累加器 B 使用 
     
     
    TIE_C0I = 0; //interrupt disable      TIE_C2I = 1;//interrupt able 
    //TIE_C1I=1; 
    TIE_C1I=0; 
     
   
    TC2 =2500;// 10ms 中断一次 
        TFLG1_C2F = 1;//set bits cause corresponding bits to be cleared 
 
     PBCTL =0x40;  //开16 位累加器 
     PACN10=0; 
      
      
} 
 
 
 
void initialATD(void){ 
   ATD0CTL2=0x40; //AD模块不上电, 快速清零,  无等待模式,  禁止外部触发, 中断静止
  ATD0CTL3=0x08; //转换序列长度为 1，不映射到结果寄存器，Freeze 模式下继续转换
  //ATD0CTL4=0x81;  //8 位精度， 2 个 A/D 时钟周期采样时间
//间,ATDClock=[BusClock*0.5]/[PRS+1]=2MHZ  busclock=8M  
  ATD0CTL4=0x87;//24M 
  ATD0CTL5=0x80; //右对齐无符号，单通道采样，单次转换 
  ATD0DIEN=0x00; //禁止数字输入  
   
  ATD1CTL2=0x40; //AD 模块不上电, 快速清零,  无等待模式,  禁止外部触发, 中断
  ATD1CTL3=0x08; //转换序列长度为 1，不映射到结果寄存器，Freeze 模式下继续
  //ATD1CTL4=0x81;  //8 位精度， 2 个 A/D 时钟周期采样时?
  //间,ATDClock=[BusClock*0.5]/[PRS+1]=2MHZ 
  ATD1CTL4=0x87;//24M   
  ATD1CTL5=0x80; //右对齐无符号，单通道采样 ，单次转换  
  ATD1DIEN=0x00; //禁止数字输入 
 
} 
 
void CheckStart(void){ 
int i,sum=0,backnum=0; 
 if(percent[10]>50 &&percent[14]>50) { 
   if(flag==0){ 
     
       TIE_C1I=1;    TC1=TCNT+4000; 
    TFLG1_C1F=1; 
    flag=1; 
  } 
    for(i=11;i<14;i++) 
      sum+=percent[i]; 
    
     if(sum<200&&sum>90) isstart++; 
     if(sum>=220)iscrossing++; 
                                                        
  } 
} 
        
  
 
uchar output_mfs[5]={0};//输出速度隶属度(单值) 
const uchar input_mfs[8][4]={ 
  0,40,0,4, 
  0,80,4,4, 
  40,120,6,6, 
  80,160,4,4, 
  120,160,4,0, 
   
  0,30,0,4, 
  15,60,8,8, 
  30,60,4,0 
   
   
  } ; 
  
const uchar rule_start[76]={             //规则表 
  (5*0)+0,(5*1)+0,0xfe,(5*0)+0+8,0xfe,  //1      
  (5*0)+0,(5*1)+1,0xfe,(5*0)+1+8,0xfe,  //2 
  (5*0)+0,(5*1)+2,0xfe,(5*0)+2+8,0xfe,  //3 
   
  (5*0)+1,(5*1)+0,0xfe,(5*0)+1+8,0xfe,  //4    
  (5*0)+1,(5*1)+1,0xfe,(5*0)+2+8,0xfe,   //5 
  (5*0)+1,(5*1)+2,0xfe,(5*0)+3+8,0xfe,   //6 
   
  (5*0)+2,(5*1)+0,0xfe,(5*0)+3+8,0xfe,   //7   
  (5*0)+2,(5*1)+1,0xfe,(5*0)+4+8,0xfe,   //8      (5*0)+2,(5*1)+2,0xfe,(5*0)+3+8,0xfe,   //9 
   
  (5*0)+3,(5*1)+0,0xfe,(5*0)+3+8,0xfe,     //10   
  (5*0)+3,(5*1)+1,0xfe,(5*0)+2+8,0xfe,   //11        
  (5*0)+3,(5*1)+2,0xfe,(5*0)+1+8,0xfe,   //12         
   
  (5*0)+4,(5*1)+0,0xfe,(5*0)+2+8,0xfe,   //13 
  (5*0)+4,(5*1)+1,0xfe,(5*0)+1+8,0xfe,   //14 
  (5*0)+4,(5*1)+2,0xfe,(5*0)+0+8,0xfe,    //15 
  0xff 
}; 
uchar fuz_ins[8];   //输入量化 
uchar fuz_outs[5];    //输出量化 
 
int fuzzy(void){ 
  uchar cog_out; 
  uchar current_ins1,current_ins2; 
  int m1,m2; 
  m1=abs(distance); 
  m2=abs(dif1); 
  current_ins1=(uchar)m1; 
  if(abs(m2)>120)m2=120; 
  current_ins2=(uchar)m2; 
   
  asm{ 
      fuzzyify: ldx #input_mfs 
              ldy #fuz_ins 
              ldaa current_ins1 
              ldab #5 
      grad_loop: mem 
              dbne b,grad_loop 
              ldaa current_ins2 
              ldab #3 
      grad_loop1:mem 
               dbne b,grad_loop1 
               ldab #5 
      rule_eval: clr 1,y+ 
               dbne b,rule_eval 
               ldx #rule_start 
               ldy #fuz_ins 
               ldaa #0xff  
               rev 
      defuz: ldy #fuz_outs 
               ldx #output_mfs 
               ldab #5 
               wav 
               ediv 
               tfr y,d 
               stab cog_out // cog_out  系统输出 
    } 
     
  return cog_out; 
} 
 
 //++ 
uchar output_mfs1[5]={0,15,30,45,60};//输出速度隶属度(单值) 
const uchar input_mfs1[8][4]={ 
  0,100,5,5, 
  50,150,5,5, 
  100,200,5,5, 
  150,240,5,6, 
  200,240,6,0, 
   
  0,60,0,4, 
  30,90,8,8, 
  60,120,4,0 
   
   
  } ; 
  
const uchar rule_start1[76]={              
  (5*0)+0,(5*1)+0,0xfe,(5*0)+0+8,0xfe,  //1      
  (5*0)+0,(5*1)+1,0xfe,(5*0)+0+8,0xfe,  //2 
  (5*0)+0,(5*1)+2,0xfe,(5*0)+1+8,0xfe,  //3 
   
  (5*0)+1,(5*1)+0,0xfe,(5*0)+1+8,0xfe,  //4    
  (5*0)+1,(5*1)+1,0xfe,(5*0)+2+8,0xfe,   //5 
  (5*0)+1,(5*1)+2,0xfe,(5*0)+3+8,0xfe,   //6 
   
  (5*0)+2,(5*1)+0,0xfe,(5*0)+2+8,0xfe,   //7  
  (5*0)+2,(5*1)+1,0xfe,(5*0)+3+8,0xfe,   //8    
  (5*0)+2,(5*1)+2,0xfe,(5*0)+3+8,0xfe,   //9    
  (5*0)+3,(5*1)+0,0xfe,(5*0)+3+8,0xfe,     //10    
  (5*0)+3,(5*1)+1,0xfe,(5*0)+4+8,0xfe,   //11        
  (5*0)+3,(5*1)+2,0xfe,(5*0)+4+8,0xfe,   //12         
   
  (5*0)+4,(5*1)+0,0xfe,(5*0)+4+8,0xfe,   //13 
  (5*0)+4,(5*1)+1,0xfe,(5*0)+4+8,0xfe,   //14 
  (5*0)+4,(5*1)+2,0xfe,(5*0)+4+8,0xfe,    //15 
  0xff 
}; 
uchar fuz_ins1[8];   //输入量化 
uchar fuz_outs1[5];    //输出量化 
 
int fuzzy1(void){ 
  uchar cog_out; 
  uchar current_ins1,current_ins2; 
  int m1,m2; 
   
  m1=abs(distance); 
  m2=abs(dif1); 
  current_ins1=(uchar)m1; 
   
  if(abs(m2)>120)m2=120; 
  current_ins2=(uchar)m2; 
   
  asm{ 
      fuzzyify: ldx #input_mfs1 
              ldy #fuz_ins1 
              ldaa current_ins1 
              ldab #5 
      grad_loop: mem 
              dbne b,grad_loop 
              ldaa current_ins2 
              ldab #3 
      grad_loop1:mem 
               dbne b,grad_loop1 
               ldab #5 
      rule_eval: clr 1,y+ 
               dbne b,rule_eval 
               ldx #rule_start1 
               ldy #fuz_ins1                
               ldaa #0xff 
               rev 
      defuz: ldy #fuz_outs1 
               ldx #output_mfs1 
               ldab #5 
               wav 
               ediv 
               tfr y,d 
               stab cog_out // cog_out  系统输出 
    } 
     
  return cog_out; 
} 
 
 
//++ 
 typedef struct PID         //定义数法核心数据 
{ 
  signed int vi_Ref;        //速度 PID，速度设定值 
 signed int vi_FeedBack;  //速度 PID，速度反馈值 
  
  
 signed long vi_PreError;  //速度 PID，前一次，速度误差,,vi_Ref - vi_FeedBack 
 signed long vi_PreDerror; //速度 PID，前一次，速度误差之差，d_error-PreDerror; 
  
  
  unsigned int v_Kp;       //速度 PID，Ka = Kp 
  unsigned int v_Ki;       //速度 PID，Kb = Kp * ( T / Ti ) 
  unsigned int v_Kd;       //速度 PID， 
     
 signed long vl_PreU;    //电机控制输出值 
   
}PID; 
 
PID  sPID;  
 
void PIDInit(void) 
{  
  sPID.vi_Ref = 0 ;                 
 sPID.vi_FeedBack = 0 ;     
  sPID.vi_PreError = 0 ;       sPID.vi_PreDerror = 0 ;       
 sPID.v_Kp = VV_KPVALUE; 
 sPID.v_Ki = VV_KIVALUE; 
 sPID.v_Kd = VV_KDVALUE; 
   
  sPID.vl_PreU = 0 ;        
}                   
  
 
unsigned int v_PIDCalc( PID *pp ) 
{ 
 signed long  error1,d_error,dd_error;  
    
    error1 = (signed long)(pp->vi_Ref - pp->vi_FeedBack);  //  偏差计算 
  
  d_error = error1 - pp->vi_PreError; 
    dd_error = d_error - pp->vi_PreDerror; 
    pp->vi_PreError = error1;  //存储当前偏差 
  pp->vi_PreDerror = d_error; 
  if( ( error1 < VV_DEADLINE ) && ( error1 > -VV_DEADLINE ) );  
 else              
   pp->vl_PreU += (signed long)(  pp -> v_Kp * d_error + pp ->  v_Ki * error1
+pp->v_Kd*dd_error); 
   
  if( pp->vl_PreU >= VV_MAX )       
  pp->vl_PreU = VV_MAX; 
   
  else if( pp->vl_PreU <= VV_MIN )     
  pp->vl_PreU = VV_MIN;    
      return (unsigned int)(pp->vl_PreU);       
} 
 
 
 
unsigned char FoundMax(void)                  
 { 
   unsigned char i,j=0; 
   max=0; 
   min=0; 
   for(i=1;i<10;i++)  
    //for(i=2;i<10;i++)     
    { 
   if(percent[max]<percent[i])  
            max=i; 
  if(percent[min]>percent[i])  
            min=i; 
   if(percent[i]>60) j++; 
   } 
    
  if(max==1) 
      PORTB=0b11111110; 
     else if(max==2) 
      PORTB=0b11111101;         
     else if(max==3)