arithmatic.h

这是本人参加FREESCAL最牛的智能小车程序

   signed char speed_tab[3][16]=       //FUZZY TABLE  2
  {
   8,7,7,6,5,4,3,2,0,0,0,-1,-2,-3,-5,-6,
   8,7,6,6,4,4,2,0,0,0,-2,-2,-3,-4,-6,-7,
   8,6,6,4,3,1,1,0,0,-2,-2,-3,-4,-5,-6,-7
  };
 
 /*------------------MOTOR CONTROL FUZZY TABLE--------------------------*/
 signed char adjust_tb[3][5]=
  {
  5,3,1,-2,-3,
  4,2,0,-3,-4,
  4,2,0,-3,-5
  };    
 /*-------------------encode fuzzy------------------------------------*/
 void encode_fuzzy()
  {
   //if(PTH==0x03&&PORTA==0xff)      SE = 0 ;  //all sensors detect white
   if(PORTAD0==0xfe)               SE = -11;  //L1低电平有效  left
   if(PORTAD0==0xfc)               SE = -10 ;   //L1,2
   if(PORTAD0==0xfd)               SE = -9 ;   //L2
   if(PORTAD0==0xfd&&PORTA==0x7f)  SE = -8 ;   //L2,3
   if(PORTA==0x7f)                 SE = -7 ;  //L3
   if(PORTA==0x3f)                 SE = -6 ;  //L3,4
   if(PORTA==0xbf)                 SE = -5 ;  // L4
   if(PORTA==0x9f)                 SE = -4 ;  //L4,5
   if(PORTA==0xdf)                 SE = -3 ;  //L5
   if(PORTA==0xcf)                 SE = -2 ;  //L5,6
   if(PORTA==0xef)                 SE = -1 ;  //L6
   if(PORTA==0xe7)                 SE = 0 ;  //L6,7        middle position
   if(PORTA==0xf7)                 SE = 1 ;   //L7          right
   if(PORTA==0xf3)                 SE = 2 ;   //L7,8
   if(PORTA==0xfb)                 SE = 3 ;  //L8
   if(PORTA==0xf9)                 SE = 4 ;  //L8,9
   if(PORTA==0xfd)                 SE = 5 ;  //L9  
   if(PORTA==0xfc)                 SE = 6 ;  //L9,10
   if(PORTA==0xfe)                 SE = 7 ;  //L10      //?
   if(PORTA==0xfe&&PORTAD0==0xfb)  SE = 8 ;   //L10,11
   if(PORTAD0==0xfb)               SE = 9 ;  //L11
   if(PORTAD0==0xf3)               SE = 10;   //L11,12
   if(PORTAD0==0xf7)               SE = 11;   //L12
   //if((PORTA==PWMDTY01)&&(PTH==0xfc)&&(PTH==0xf3))  {SE=0;stop++;}
  }
 ///*********************FUZZY CONTROL JUSTIFY****************************///
void fuzzy_just()
  {
   encode_fuzzy();
   if(ME>8||ME<-8)  encode_fuzzy();   //new encode to clear disturbance
   ME = SE-last_SE;
   store_SE[0] = SE;
   last_SE = SE;
   for(SE_cnt=8;SE_cnt>0;SE_cnt--)
   store_SE[SE_cnt+1]=store_SE[SE_cnt];
   store_SE[1]=store_SE[0];
   if(!ME)      MET = 1;              //NS;
   else if(ME<0)     MET = 0;         //ZO;
   else  MET = 2;                     //PS;
   switch (SE) 
    {
      case -11 :  SET = 0;      break;
      case -10 :  SET = 1;      break;
      case -9 :   SET = 2;      break;
      case -8 :   SET = 3;      break;
      case -7 :   SET = 4;      break;
      case -6 :   SET = 5;      break;
      case -5 :   SET = 6;      break;
      case -4 :   SET = 7;      break;
      case -3 :   SET = 8;      break;
      case -2 :   SET = 9;      break;
      case -1 :   SET = 10;      break;
      case 0 :    SET = 11;      break;
      case 1 :    SET = 13;      break;
      case 2 :    SET = 12;      break;
      case 3 :    SET = 14;      break;
      case 4 :    SET = 15;      break;
      case 5 :    SET = 16;      break;
      case 6 :    SET = 17;      break;
      case 7 :    SET = 18;      break;
      case 8 :    SET = 19;      break;
      case 9 :    SET = 20;      break;
      case 10 :   SET = 21;      break;
      case 11 :   SET = 22;      break;  
    }
    //d_angel = SET-last_SET;
    //last_SET = SET;
   }
 /*****************the justification of road conditions****************////
 void road_just()       //?simple??
  {
   signed  char i; //j;
    if((-8<=SE)&&(SE<=-2)||(2<=SE)&&(SE<=8)&&(PORTAD0_PTAD4==0||PORTAD0_PTAD5==0))     //another method slike 
    {
      road_state = s_like;
      set_speed = slike_speed;
    }
   if((PORTAD0_PTAD4==0||PORTAD0_PTAD5==0)&&((-2<=SE)&&(SE<=2))) 
   {
    road_state = line; 
    set_speed = line_speed;
   }
   else
    {
   i = SE*last_SE;           
   if(i<0) 
   {
    road_state = s_like;  //有一次正负交替为蛇形道  ?too simple??
    set_speed = slike_speed;
   }
   else  //if(just_flag>=9)
   {
    curve_just();    //弯道
   }
    }
   /*for(j=0;j<10;j++)                 //another method slike 
     just_s+=store_SE[j];
   if(just_s<16&&just_s>-16) 
    {
      road_state=s_like;
      set_speed = b_speed; 
    } */
    
   
  }
/*********************curve just formular***********************////
void curve_just()           //离散化  //? simple??
  {
   signed int degree_curve;
   if(((3<=SE)&&(SE<=8))||((-8<SE)&&(SE<=-3)))  
      degree_curve =0;              //  L8,13,14small curve
   if(((8<=SE)&&(SE<=11))||((-11<=SE)&&(SE<=-8)))   
      degree_curve =8;
   if(degree_curve>big_curve)   
   {
    road_state =  curve_b;
    set_speed = bc_speed;
   }
   else  
    {
     road_state = curve_s; 
     set_speed = sc_speed; 
    }
  }
  /*----------------speed control()-----------------------------*/
void speed_control()
  {
  if(servo>=-2&&servo<=2)   {NB=-8;NS=-6;PS=6;PB=8;}      //尝试离散化精确控制？？？
  if(servo==-3&&servo==3)   {NB=-8;NS=-6;PS=6;PB=8;}
  if(servo==-4&&servo==4)   {NB=-6;NS=-4;PS=5;PB=6;}
  if(servo==-5&&servo==5)   {NB=-4;NS=-4;PS=4;PB=6;}
  if(servo==-6&&servo==6)   {NB=-4;NS=-2;PS=2;PB=4;}   
  EK = cur_speed-set_speed; 
  DEK = EK-last_EK;                       //DEK
  last_EK = EK;
  if(EK<-21)            ekt=0;  
  if(EK>=-21&&EK<=-19)  ekt=1;
  if(EK>=-18&&EK<=-16)  ekt=2;
  if(EK>=-15&&EK<=-13)  ekt=3;
  if(EK>=-12&&EK<=-10)  ekt=4;
  if(EK>=-9&&EK<=-7)    ekt=5;
  if(EK>=-6&&EK<=-4)    ekt=6;
  if(EK>=-3&&EK<=0)     ekt=7;
  if(EK>=1&&EK<=3)      ekt=8;
  if(EK>=4&&EK<=6)      ekt=9;
  if(EK>=7&&EK<=9)      ekt=10;
  if(EK>=10&&EK<=12)    ekt=11;
  if(EK>=13&&EK<=15)    ekt=12;
  if(EK>=16&&EK<=18)    ekt=13;
  if(EK>=19&&EK<=21)    ekt=14;
  if(EK>21)             ekt=15;
   if(DEK<=NB)   dekt = 0;
   if(DEK>=NS&&DEK<=PS)   dekt = 1;
   if(DEK>=PB)   dekt = 2;
   if(PWMDTY2>=200) PWMDTY2=150;
   if(PWMDTY2<+90) 
    {
    PWMDTY2=90;
    set_speed=13;
    }
   if(PWMDTY2<=200&&PWMDTY2>=90)
   PWMDTY2=PWMDTY2+3*speed_tab[dekt][ekt];
   //PWMDTY2=PWMDTY2+5*adjust_tb[dekt][ekt];
  }