Wataru Mikawa / Mbed 2 deprecated Kalman_test

MPU6050を用いた姿勢推定

Dependencies:   Kalman MPU6050 mbed

main.cpp@0:fdabd560722e, 2018-01-19 (annotated)

Committer:
mikawataru
Date:
Fri Jan 19 10:43:08 2018 +0000
Revision:
0:fdabd560722e
initialize

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 0:fdabd560722e 1 #include "mbed.h"
mikawataru 0:fdabd560722e 2 #include "math.h"
mikawataru 0:fdabd560722e 3 #include "MPU6050.h"
mikawataru 0:fdabd560722e 4 #include "Kalman.h"
mikawataru 0:fdabd560722e 5
mikawataru 0:fdabd560722e 6 #define RadToDeg 57.295779513082320876798154814105
mikawataru 0:fdabd560722e 7 MPU6050 mpu(p9,p10);
mikawataru 0:fdabd560722e 8 Serial pc(USBTX, USBRX);
mikawataru 0:fdabd560722e 9 KalmanFilter gKfx, gKfy;
mikawataru 0:fdabd560722e 10 Timer timer;
mikawataru 0:fdabd560722e 11
mikawataru 0:fdabd560722e 12 float gCalibrateX; // 初期化時の角度。（＝静止角とみなす）
mikawataru 0:fdabd560722e 13 float gCalibrateY;
mikawataru 0:fdabd560722e 14 float gPrevMicros;
mikawataru 0:fdabd560722e 15
mikawataru 0:fdabd560722e 16 int main() {
mikawataru 0:fdabd560722e 17 timer.start();
mikawataru 0:fdabd560722e 18 float a[3];
mikawataru 0:fdabd560722e 19 float g[3];
mikawataru 0:fdabd560722e 20
mikawataru 0:fdabd560722e 21 pc.baud(115200);
mikawataru 0:fdabd560722e 22 mpu.setAcceleroRange(2);
mikawataru 0:fdabd560722e 23 mpu.setGyroRange(2);
mikawataru 0:fdabd560722e 24
mikawataru 0:fdabd560722e 25 mpu.getAccelero(a);
mikawataru 0:fdabd560722e 26 mpu.getGyro(g);
mikawataru 0:fdabd560722e 27 float degRoll = atan2(a[1], a[2]) * RadToDeg;
mikawataru 0:fdabd560722e 28 float degPitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
mikawataru 0:fdabd560722e 29
mikawataru 0:fdabd560722e 30 gKfx.setAngle(degRoll);
mikawataru 0:fdabd560722e 31 gKfy.setAngle(degPitch);
mikawataru 0:fdabd560722e 32 gCalibrateY = degPitch;
mikawataru 0:fdabd560722e 33 gCalibrateX = degRoll;
mikawataru 0:fdabd560722e 34 gPrevMicros = timer.read();
mikawataru 0:fdabd560722e 35
mikawataru 0:fdabd560722e 36 while(1) {
mikawataru 0:fdabd560722e 37 mpu.getAccelero(a);
mikawataru 0:fdabd560722e 38 mpu.getGyro(g);
mikawataru 0:fdabd560722e 39 degRoll = atan2(a[1], a[2]) * RadToDeg;
mikawataru 0:fdabd560722e 40 degPitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
mikawataru 0:fdabd560722e 41
mikawataru 0:fdabd560722e 42 float dpsX = g[0] * RadToDeg;
mikawataru 0:fdabd560722e 43 float dpsY = g[1] * RadToDeg;
mikawataru 0:fdabd560722e 44 float dpsZ = g[2] * RadToDeg;
mikawataru 0:fdabd560722e 45
mikawataru 0:fdabd560722e 46 float curMicros = timer.read();
mikawataru 0:fdabd560722e 47 float dt = curMicros - gPrevMicros;
mikawataru 0:fdabd560722e 48 gPrevMicros = curMicros;
mikawataru 0:fdabd560722e 49
mikawataru 0:fdabd560722e 50 float degX = gKfx.calcAngle(degRoll, dpsX, dt);
mikawataru 0:fdabd560722e 51 float degY = gKfy.calcAngle(degPitch, dpsY, dt);
mikawataru 0:fdabd560722e 52 degY -= gCalibrateY;
mikawataru 0:fdabd560722e 53 degX -= gCalibrateX;
mikawataru 0:fdabd560722e 54
mikawataru 0:fdabd560722e 55 pc.printf("%f,%f,%f\r\n",dt,-degX,degY);
mikawataru 0:fdabd560722e 56 wait(0.001);
mikawataru 0:fdabd560722e 57 }
mikawataru 0:fdabd560722e 58 }