制作平衡车-MPU6050实验
本帖最后由 zaqwsx626 于 2014-3-30 22:18 编辑今晚用了一些时间试了下6050，参考了一下黑马的程序
我的自平衡小车D2
和飞思卡尔直立车方案
上代码
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#define pi 3.14159
#define k 40//比例
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t ax0, ay0, az0;
int16_t gx0, gy0, gz0;
unsigned long time;
float angleG;
void setup()
{
//接入i2c总线
Wire.begin();
Serial.begin(115200);
//初始化设备
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
//链接设备
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
time=millis();
offset();//测出陀螺仪偏移量，加计？
Serial.println(millis()-time);
}
void offset()
{
accelgyro.getMotion6(&ax0, &ay0, &az0, &gx0, &gy0, &gz0);
for(int i=0;i<1000;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//ax0=(ax+ax0)/2;
//ay0=(ay+ay0)/2;
//az0=(az+az0)/2;
gx0=(gx+gx0)/2;
gy0=(gy+gy0)/2;
gz0=(gz+gz0)/2;
}
}
void loop()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
int dt=millis();
//Serial.print(ax); Serial.print(",");
//Serial.print(ay); Serial.print(",");
//Serial.print(az); Serial.print(",");
//x,y,z轴角速度
//Serial.print(gx-gx0); Serial.print(",");
//Serial.print(gy-gy0); Serial.print(",");
//Serial.println(gz-gz0);
float AccX=ax;
float AccZ=az;
float angleA = atan(AccZ / AccX) * 180 / pi;//加计计算
Serial.print(angleA);Serial.print(",");
float GyrY=gy - gy0;//减去偏移量
dt=millis()-dt;//积分时间
float Da=angleA-angleG;//加计算出的角与陀螺积分叫做差
angleG = angleG + (k*Da+GyrY/131) * dt / 1000;//陀螺积分
Serial.println(angleG);
}
angleG = angleG + (k*Da+GyrY/131) * dt / 1000; 差值Da乘比例k后参与积分
依旧有问题：
angleG = angleG + (GyrY/131) * dt / 1000; 这句是原来参考黑马程序改的，可是不知为什么得到的角度与加计得出的角度相差很多，因此想到了飞思卡尔方案中的方法，结果看来还不错。
加速度计该怎么校准，总不能重力去除吧，毕竟用他计算了角度。
SerialChart即时性好像不怎么强，波形跟不上，该怎么解决? 嘿嘿，被楼主的图给晃了。好平滑的曲线。仔细看程序，这个滤波是读了很多次，然后用新数值和之前的求平均值？好像最后几次的数值对结果影响比较大？ shihaipeng04 发表于 2014-4-1 01:03 static/image/common/back.gif
嘿嘿，被楼主的图给晃了。好平滑的曲线。仔细看程序，这个滤波是读了很多次，然后用新数值和之前的求平均值 ...
取陀螺仪零飘时确实是多次求平均，至于滤波倒不是求平均。学生一枚，对于滤波这些东西也不太懂，请多指点。 zaqwsx626 发表于 2014-4-1 16:51 static/image/common/back.gif
取陀螺仪零飘时确实是多次求平均，至于滤波倒不是求平均。学生一枚，对于滤波这些东西也不太懂，请多指点 ...
最近也在看各种滤波，我觉得还是学生好，学过的还没忘太多，没学的还有机会。 像我这个早就毙了业的，啥也不会了。诶
页:
[1]