平衡直立车要想保证直立,得知当前角度和角速度尤为重要;
加速度计常用两种:数字加速度计和模拟加速度计;
数字加速度计一般通过I2C来传送数据,模拟加速度计直接给出电压读数;
模拟加速度计使用较为简单,而且读取速度和精度基本能满足平衡直立车的需求;
数字加速度计可以进行丰富的配置,获得更加理想的数据而不耗费单片机过多资源。
陀螺仪用来测量角速度。这里用的是模拟陀螺仪,直接用ADC读数。
计算角度的方法
牛顿老人家说过: F = ma; 加速度计能测量加速度a,从而可以得知测量方向上所受的重力分量;
测出小车前后水平放置时,加速度计的读数,可以得到±G的读数。小车行走时,加速度计的读数除以G的读数,就能得到角度值了。
具体请参考互补滤波算法:https://feichashao.com/balance_filter/
K60
K60用IIC读取数字加速度计。
详细的配置方法,请参考MMA8451文档。
这里的代码不是我写的(学长大大写的~),所以我不能全贴出来,只好挑一些讲讲原理咯。
I2C_init(MMA845xI2C); //初始化I2C0 MMA845x_WRITE_BYTE(CTRL_REG1,ASLP_RATE_20MS+DATA_RATE_5MS); MMA845x_WRITE_BYTE(XYZ_DATA_CFG_REG, FULL_SCALE_2G); //设定测量范围为2G MMA845x_WRITE_BYTE(CTRL_REG1, ACTIVE_MASK); //设为激活状态
就是这样,使用前要配置好参数。
读取读数可以这么干
u16 Get845xY_12bit(u8 N) { u32 tmp = 0; u8 i = 0; u16 ym = 0; u16 yl = 0; for(i = 0; i < N; i++) { ym = MMA845x_READ_BYTE(OUT_Y_MSB_REG); yl = MMA845x_READ_BYTE(OUT_Y_LSB_REG); tmp += ( ((((ym<<8) & 0xFF00 ) | (yl&0x00FF)) >> 4) & 0x0FFF); } tmp = tmp / N; return (u16)tmp; }
由于陀螺仪存在温漂,所以启动的时候,要采集一次角速度为0的Base value。最后在小车启动后一秒再采集这个零偏值,因为刚启动的时候可能有抖动,会影响读数。
陀螺仪用ADC读数,所以第一步是初始化ADC,以后直接读AD即可。
//ENC 03 初始化 PE3 adc_init(ENC_AD,ENC_X_CH);
读零偏值
void gyro_init() { delayms(2000); gyro_stand_voltage = ad_ave(ENC_AD,ENC_X_CH,ADBITS,30); //直立陀螺仪零偏置 12bits 2000 400 }
XS128
在XS128中,我们陀螺仪和加速度计都是用模拟的,所以配置ADC即可。
详细请参考, Chapter 10 Analog-to-Digital Converter, MC9S12XS256 Reference Manual.
直接AD读数就能获取角度和角速度啦。 void GetVoltage() //获得电压平均值 { char i; ulong temp_gyro=0,temp_acce=0; for(i=0;i<31;i++)//31次平均 { temp_gyro += ReadATD(CHANNEL_GYRO); temp_acce += ReadATD(CHANNEL_ACCE); } GYRO_Vol = temp_gyro/64.0;//参考电压4.96V,496/1023/31 = /64 ACCE_Vol = temp_acce/64.0; }