Wataru Mikawa
MPU6050を用いた姿勢推定
Dependencies: Kalman MPU6050 mbed
Revision 0:fdabd560722e, committed 2018-01-19
- Comitter:
- mikawataru
- Date:
- Fri Jan 19 10:43:08 2018 +0000
- Commit message:
- initialize
Changed in this revision
diff -r 000000000000 -r fdabd560722e Kalman.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman.lib Fri Jan 19 10:43:08 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mikawataru/code/Kalman/#3e14df89021f
diff -r 000000000000 -r fdabd560722e MPU6050.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Fri Jan 19 10:43:08 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MPU6050/#5c63e20c50f3
diff -r 000000000000 -r fdabd560722e main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jan 19 10:43:08 2018 +0000
@@ -0,0 +1,58 @@
+#include "mbed.h"
+#include "math.h"
+#include "MPU6050.h"
+#include "Kalman.h"
+
+#define RadToDeg 57.295779513082320876798154814105
+MPU6050 mpu(p9,p10);
+Serial pc(USBTX, USBRX);
+KalmanFilter gKfx, gKfy;
+Timer timer;
+
+float gCalibrateX; // 初期化時の角度。（＝静止角とみなす）
+float gCalibrateY;
+float gPrevMicros;
+
+int main() {
+ timer.start();
+ float a[3];
+ float g[3];
+
+ pc.baud(115200);
+ mpu.setAcceleroRange(2);
+ mpu.setGyroRange(2);
+
+ mpu.getAccelero(a);
+ mpu.getGyro(g);
+ float degRoll = atan2(a[1], a[2]) * RadToDeg;
+ float degPitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
+
+ gKfx.setAngle(degRoll);
+ gKfy.setAngle(degPitch);
+ gCalibrateY = degPitch;
+ gCalibrateX = degRoll;
+ gPrevMicros = timer.read();
+
+ while(1) {
+ mpu.getAccelero(a);
+ mpu.getGyro(g);
+ degRoll = atan2(a[1], a[2]) * RadToDeg;
+ degPitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
+
+ float dpsX = g[0] * RadToDeg;
+ float dpsY = g[1] * RadToDeg;
+ float dpsZ = g[2] * RadToDeg;
+
+ float curMicros = timer.read();
+ float dt = curMicros - gPrevMicros;
+ gPrevMicros = curMicros;
+
+ float degX = gKfx.calcAngle(degRoll, dpsX, dt);
+ float degY = gKfy.calcAngle(degPitch, dpsY, dt);
+ degY -= gCalibrateY;
+ degX -= gCalibrateX;
+
+ pc.printf("%f,%f,%f\r\n",dt,-degX,degY);
+ wait(0.001);
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r fdabd560722e mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Jan 19 10:43:08 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/7130f322cb7e \ No newline at end of file