怎样去设计一种基于AVR单片机的两轮自平衡智能车

　　所需原件：
　　AVR单片机 ： 采用之前的ATmega128开发板
　　传感器： ENC03陀螺仪 mma8451加速度计
　　电机驱动： L298n
　　总体思路：
　　使用陀螺仪测角速度，使用加速度计测角度，使用测得的这两个值进行传感器融合，获取准确的角度。由得到的角度来控制车子两个轮子的转向，转速等参数。进而实现两轮车的平衡。 传感器融合使用卡尔曼滤波算法，不得不大赞一下卡尔曼同学，给了个这么perfect的算法，传说当年挑战者号就是用的这个算法。
　　角度的测量：
　　enc03陀螺仪： 采用的这个陀螺仪是模拟量的，输出电压需要用ATmega128进行adc转换成数字量，测得电压，再转换成角速度：
　　ISR（ADC_vect）{
　　if（adc_done_flag==0）{
　　adc_read（adc_channel）;
　　if（adc_channel==1） {
　　adc_channel=0;
　　}
　　else{
　　adc_channel++;
　　adc_mid_tmp0［cou］=adc_value［0］;
　　//中值滤波
　　cou++;
　　if（cou==3）{
　　cou=0;
　　if（adc_mid_tmp0［0］》adc_mid_tmp0［1］）{
　　mid_tmp = adc_mid_tmp0［0］;
　　adc_mid_tmp0［0］ =adc_mid_tmp0［1］;
　　adc_mid_tmp0［1］ =mid_tmp;
　　}
　　if（adc_mid_tmp0［2］》adc_mid_tmp0［1］）
　　adc_mid［0］=adc_mid_tmp0［1］ ;
　　else if （adc_mid_tmp0［2］》adc_mid_tmp0［0］）
　　adc_mid［0］=adc_mid_tmp0［2］ ;
　　else
　　adc_mid［0］=adc_mid_tmp0［0］ ;
　　}
　　}
　　adc_set_channel（adc_channel）;
　　adc_done_flag =1;
　　}
　　else adc_done_flag = 0;
　　//ADCSRA|=（1《《ADSC）;
　　}
　　float gyro_enc03_w（unsigned int adc_value，float adc_bias）{
　　float w_value;
　　//把adc采集的值转化为电压
　　w_value = （float）（adc_value）*2540/1024; //mV
　　w_value = w_value-adc_bias;
　　//把电压转化为角速度
　　w_value = -w_value;
　　w_value = w_value/0.67;
　　return w_value;
　　}
　　mma8451加速度计： mma8451使用的I2C接口，不过没事，128上面就有一个了，这里我使用硬件I2C来实现，代码较多这里给出关键代码：
　　unsigned char i2c_mma8451_Write（unsigned char Address，unsigned char Wdata）
　　{
　　unsigned char i;
　　Start（）;//I2C启动
　　Wait（）;
　　if（TestAck（）！=START） return 1;//ACK
　　Write8Bit（MMA845x_IIC_ADDRESS）;//写I2C 8451从器件地址和写方式
　　Wait（）;
　　if（TestAck（）！=MT_SLA_ACK） return 1;//ACK
　　Write8Bit（Address）;//写x地址
　　Wait（）;
　　if（TestAck（）！=MT_DATA_ACK） return 1;//ACK
　　Write8Bit（Wdata）;//写入设备ID及读信
　　Wait（）;
　　if（TestAck（）！=MT_DATA_ACK） return 1;//ACK
　　Stop（）;//I2C停止
　　for（i=0;i《250;i++）;
　　return 0;
　　}
　　//mma8451初始化
　　//初始化为指定模式
　　void mma845x_init（）
　　{ i2c_init（）;
　　i2c_mma8451_Write（CTRL_REG1，ASLP_RATE_20MS+DATA_RATE_5MS）;
　　i2c_mma8451_Write（XYZ_DATA_CFG_REG， FULL_SCALE_2G）; //2G
　　i2c_mma8451_Write（CTRL_REG1， （ACTIVE_MASK+ASLP_RATE_20MS+DATA_RATE_5MS）&（~FREAD_MASK））; //激活状态 14bit
　　}
　　//
　　unsigned char i2c_mma8451_Read（unsigned char Address）
　　{
　　unsigned char temp;
　　Start（）;//I2C启动
　　Wait（）;
　　if （TestAck（）！=START） return 0;//ACK
　　Write8Bit（MMA845x_IIC_ADDRESS）;//写入设备ID及写信号
　　Wait（）;
　　if （TestAck（）！=MT_SLA_ACK） return 0;//ACK //
　　//
　　Write8Bit（Address）;//写X地址
　　Wait（）;
　　if （TestAck（）！=MT_DATA_ACK） return 0; //？
　　//data1=TestAck（）;
　　Start（）;//I2C重新启动
　　Wait（）;
　　if （TestAck（）！=RE_START） return 0;
　　Write8Bit（MMA845x_IIC_ADDRESS+1）;//写I2C从器件地址和读方式
　　Wait（）;
　　if（TestAck（）！=MR_SLA_ACK） return 0;//ACK
　　Twi（）;//启动主I2C读方式
　　Wait（）;
　　if（TestAck（）！=MR_DATA_NOACK） return 0;//ACK
　　temp=TWDR;//读取I2C接收数据
　　Stop（）;//I2C停止
　　return temp;
　　}
　　//求出加速度原始对应数值
　　unsigned int get_mma8451_data（unsigned char Address）{
　　unsigned char x;
　　unsigned int wx;
　　unsigned char address_LSB;
　　x = i2c_mma8451_Read（Address）;
　　if（Address==OUT_Z_MSB_REG）
　　address_LSB= OUT_Z_LSB_REG;
　　else if（Address==OUT_Y_MSB_REG）
　　address_LSB= OUT_Y_LSB_REG;
　　else
　　address_LSB= OUT_X_LSB_REG;
　　wx = （（i2c_mma8451_Read（address_LSB））|x《《8）;
　　return wx;
　　}
　　使用卡尔曼滤波融合两个测量量：（参考自原来在阿莫论坛很流行的算法）
　　float angle， angle_dot; //外部需要引用的变量
　　//-------------------------------------------------------
　　float Q_angle=0.001， Q_gyro=0.003， R_angle=5.0， dt=0.01;
　　//注意：dt的取值为kalman滤波器采样时间;
　　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;
　　void Kalman_Filter（float angle_m，float gyro_m） //gyro_m:gyro_measure
　　{
　　angle+=（gyro_m-q_bias） * dt;//先验估计
　　Pdot［0］=Q_angle - P［0］［1］ - P［1］［0］;// Pk-‘ 先验估计误差协方差的微分
　　Pdot［1］=- P［1］［1］;
　　Pdot［2］=- P［1］［1］;
　　Pdot［3］=Q_gyro;
　　P［0］［0］ += Pdot［0］ * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
　　P［0］［1］ += Pdot［1］ * dt;
　　P［1］［0］ += Pdot［2］ * dt;
　　P［1］［1］ += Pdot［3］ * dt;
　　angle_err = angle_m - angle;//zk-先验估计
　　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;//Kk
　　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;//输出值（后验估计）的微分 = 角速度
　　}
　　卡尔曼算法是要在定时器里面不断的更新，才能发挥作用.dt就是定时器的时间，这里我采用10ms.
　　主要算法在定时器中断里面实现：
　　定时器里面完成对角速度，角度的测量，卡尔曼更新，以及对电机的控制。
　　//定时器0 初始化
　　void timer0_init（void）{
　　//设置计数开始的初始值
　　TCNT0 = 176 ; // 208》》6ms 176》》10ms
　　//设置分频
　　TCCR0 = （1《《CS02）|（1《《CS00）|（1《《CS01）; //1024分频 47次6ms 62.5次8ms
　　//设置中断屏蔽寄存器
　　TIMSK = （1《《TOIE0）; //设置溢出使能中断
　　}
　　//定时器0溢出中断
　　ISR（TIMER0_OVF_vect）{
　　//设置计数开始的初始值
　　TCNT0 = 176 ; //10ms
　　mma8451_data = get_mma8451_data（OUT_Z_MSB_REG）;
　　acc = mma8451_i2c_to_angel（mma8451_data）;
　　acc_tmp=acc;
　　gyro = gyro_enc03_w（adc_mid［0］，adc_bias）;
　　gyro_tmp = gyro;
　　//卡尔曼
　　Kalman_Filter（acc，gyro）;
　　//控制车速
　　if （angle》0） {
　　car_back（）;
　　if（angle》15）
　　car_speed（0x2f，0x2f）;
　　else
　　car_speed（0x4f，0x4f）;
　　}
　　else if（angle《0）{
　　car_forward（）;
　　if（angle《-15）
　　car_speed（0x2f，0x2f）;
　　else
　　car_speed（0x4f，0x4f）;
　　}
　　}
       }
      对于车速的控制，这里只采用简单的开环控制，之后会采用更复制的DIP控制或者模糊控制等算法。