|
|
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float Gyrox = 0,Gyroy = 0,Gyroz = 0,lasttimeX=0,lasttimeY=0,lasttimeZ=0;
float Accx = 0.00,Accy = 0.00,Accz = 0.00;
float Ax = 0.00,Ay = 0.00,Az = 0.00;
float lastAx = 0.00,lastAy = 0.00,lastAz = 0.00;
long timer = 0; // 采样时间
void setup() {
Wire.begin();
Serial.begin(9600);
accelgyro.initialize();
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
long o_timer = timer; // 上一次采样时间(ms)
Accx = (ax/8192.00 - 0.025)*9.8;
Accy = (ay/8192.00 + 0.025)*9.8;
Accz = (az/8192.00 + 0.1)*9.8;
gx = gx/131.00;
gy = gy/131.00;
gz = gz/131.00;
timer = millis(); // 当前时间(ms)
float dt = (timer - o_timer) / 1000.0; // 微分时间(s)
Gyrox = Gyrox + (lasttimeX+gx)*dt/2;
Gyroy = Gyroy + (lasttimeY+gy)*dt/2;
Gyroz = Gyroz + (lasttimeZ+gz)*dt/2;
Ax = Ax +(lastAx+ Accx)*dt/2;
Ay = Ay +(lastAy+ Accy)*dt/2;
Az = Az +(lastAz+ Accz)*dt/2;
lasttimeX = gx;
lasttimeY = gy;
lasttimeZ = gz;
lastAx = Accx;
lastAy = Accy;
lastAz = Accz;
Serial.print("GX: ");
Serial.print(Gyrox); Serial.print(" GY: ");
Serial.print(Gyroy); Serial.print(" GZ: ");
Serial.print(Gyroz); Serial.print(" AX: ");
Serial.print(Ax); Serial.print(" AY: ");
Serial.print(Ay); Serial.print(" AZ: ");
Serial.println(Az);
}
小弟想推算出速度,藉由對加速度積分所得
但是我所積分出的數值卻很奇怪,
所推的速度幾乎不會隨著移動變動...
想請問我積分是錯誤的嗎?
梯形公式來推積分的
不知道 板上是否有人成功將速度計算出來過呢?
有的話,有請大大指示一下了
|
|
发表于 2015-4-14 01:57:41
楼主