附件2：重点代码

2.1车身角度滤波代码

/************滤波************/
float P[2][2] = {{ 1, 0 },{ 0, 1 }};
float Pdot[4] ={0,0,0,0};
const char C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;
void Kalman_Filter(float angle_m,float gyro_m)  
{
 angle+=(gyro_m-q_bias) * dt;      
 Pdot[0]=Q_angle - P[0][1] - P[1][0];   
 Pdot[1]=- P[1][1];
 Pdot[2]=- P[1][1];
 Pdot[3]=Q_gyro;
 
 P[0][0] += Pdot[0] * dt;    
 P[0][1] += Pdot[1] * dt;
 P[1][0] += Pdot[2] * dt;
 P[1][1] += Pdot[3] * dt;
 angle_err = angle_m - angle;    
 
 PCt_0 = C_0 * P[0][0];
 PCt_1 = C_0 * P[1][0];
 E = R_angle + C_0 * PCt_0;
 
 K_0 = PCt_0 / E;
 K_1 = PCt_1 / E;
 
 t_0 = PCt_0;
 t_1 = C_0 * P[0][1];
 P[0][0] -= K_0 * t_0;     
 P[0][1] -= K_0 * t_1;
 P[1][0] -= K_1 * t_0;
 P[1][1] -= K_1 * t_1;
 angle   += K_0 * angle_err;  
 q_bias  += K_1 * angle_err;    
 angle_dot = gyro_m-q_bias;  
}
//**************滤波*****************//
static float C_angle,C_angle_dot; 
static float bias_cf;
void Complement_filter(float angle_m_cf,float gyro_m_cf)
{
 bias_cf=0.998*bias_cf+0.002*gyro_m_cf; 
 C_angle_dot=gyro_m_cf-bias_cf;
 C_angle=0.98*(C_angle+C_angle_dot*0.02)+0.02*angle_m_cf;
}
//***************************** 滤波结束*********************************/

2.2 转向数据处理代码

/************转向************/
void Steering_handle(void) 
{ 
  
    Buf=  0.9 *Buf + 0.1 * AD_Turn;      
    Turning= Buf -Turn_Zero; //     
    if(Turning <- Turn_Dead)        //死区
      Turning+=Turn_Dead; 
       
    else if(Turning> Turn_Dead)
      Turning-=Turn_Dead; 
       
    else Turning= 0; 
    
   if (mode==0)   
   {
    Drive_A=0;
    Drive_B=0;
    if (!(angle>0.1||angle<-0.1))
    {
     mode=1;
    }
   } 
   else
   {
    if(lab==0)
     {
      Turning=0;
     }
    else if (Turning>55||Turning<-55)//
     {
      Turning=0;
      lab=3;// turn error
     }
    else     //按车速整定转向数据
     {
     
     //buf2=Drivespeed;
     //if (buf2<0)buf2*=-1;
     //buf2/=3;
     //Turning/=buf2;
     Turning/=1;
     }
    
     Drive_A=Drivespeed-Turning;
     Drive_B=Drivespeed+Turning;
  
   }  
} 
//***************************** 转向结束*********************************/

2.3遥控部分状态机

/***********按键********/
#define BOOL int
#define FALSE 0
#define TRUE  1
#define INT8U unsigned int
/**********硬件接口***********/
 #define  KEYPIN1      (PINC&(1<<3))
 #define  KEYPIN2      (~PINB&(1<<0))
 #define  KEYPIN3      (~PINB&(1<<1))
 #define  KEYPIN4      (~PINB&(1<<3))
 #define  KEYPIN5      (~PINB&(1<<4)) 
 
/**********按恪键属性**********/
 #define KEY_JT 0x0e
 #define KEY_A  0x0d
 #define KEY_B  0x0b
 #define KEY_C  0x07
 #define KEY_D  0x08
 
 #define KEY_NULL 0x0f
//
#define KEY_LONG_PERIOD   250
#define KEY_CONTINUE_PERIOD  25
//
#define KEY_DOWN    0x80
#define KEY_LONG    0x40
#define KEY_CONTINUE   0x20
#define KEY_UP   0x10
//
#define KEY_STATE_INIT  0
#define KEY_STATE_WOBBLE   1
#define KEY_STATE_PRESS 2
#define KEY_STATE_LONG  3
#define KEY_STATE_CONTINUE 4
#define KEY_STATE_RELEASE  5
 uchar KeyScan(void)  
{
 if(KEYPIN2==0) return KEY_A;
 if(KEYPIN3==0) return KEY_B;
 if(KEYPIN4==0) return KEY_C;
 if(KEYPIN5==0) return KEY_D;
 if(KEYPIN1==0) return KEY_JT;
 return KEY_NULL;
}

void GetKey(uchar *pKeyValue)
{
  static char KeyState = KEY_STATE_INIT;
  static char KeyTimeCount = 0;
  static char LastKey = KEY_NULL; 
  char KeyTemp = KEY_NULL;
  KeyTemp = KeyScan();   
  switch(KeyState)
  {
   case KEY_STATE_INIT:
     {
      if(KEY_NULL!=(KeyTemp))
      {
      KeyState = KEY_STATE_WOBBLE;
      }
     }
    break;
    case KEY_STATE_WOBBLE: 
      {
      KeyState = KEY_STATE_PRESS;
    } 
    break;
    case KEY_STATE_PRESS:
      {
     if(KEY_NULL!=(KeyTemp))  
     {
      LastKey = KeyTemp; 
      KeyTemp|=KEY_DOWN; 
      KeyState = KEY_STATE_LONG ;
     }
     else
     {
      KeyState = KEY_STATE_INIT;
     }
    }
    break;
    case KEY_STATE_LONG:
      {
     if(KEY_NULL !=(KeyTemp))
     {
      if(++KeyTimeCount > KEY_LONG_PERIOD)
      {
        KeyTimeCount = 0;
        KeyTemp|=KEY_LONG; 
        KeyState = KEY_STATE_CONTINUE;
       }
      }
      else 
      {
        KeyState = KEY_STATE_RELEASE;
      }
    }
  break;
  case KEY_STATE_CONTINUE:
      {
      if(KEY_NULL !=(KeyTemp))
      {
       if(++KeyTimeCount > KEY_CONTINUE_PERIOD)
        {
         KeyTimeCount = 0;
         KeyTemp |= KEY_CONTINUE;
      }
      }
      else 
      {
        KeyState = KEY_STATE_RELEASE;
       }
      }
      break;
      case KEY_STATE_RELEASE:
      {
     LastKey |=KEY_UP;
     KeyTemp = LastKey;
     KeyState = KEY_STATE_INIT;
      }
      break;
      default:break;
  }
  *pKeyValue = KeyTemp;  
}   

2.4电池电压