Diff: TI_MPU6050.cpp

Revision:

1:bc1e8a435c95

Parent:

0:81e5563b1ea1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TI_MPU6050.cpp	Mon Jun 04 22:41:56 2018 +0000
@@ -0,0 +1,73 @@
+#include "TI_MPU6050.h"
+#include "mbed.h"
+
+TI_MPU6050::TI_MPU6050()
+{
+    _mpu.initialize();
+
+    bool result = _mpu.testConnection();
+
+    if (result) {
+        // printf("MPU6050 Test Connection passed\n\r");
+    } else {
+        // printf("MPU6050 Test Connection failed\n\r");
+    }
+}
+
+int TI_MPU6050::getVerticalAngle()
+{
+    int16_t ax, ay, az;
+    int16_t gx, gy, gz;
+
+    int i = 0;
+    long x = 0;
+    long y = 0;
+    long z = 0;
+
+    int repeatCount = 100;
+
+    // 各データを100回読み込んで平均化
+    for (i = 0; i < repeatCount; ++i) {
+        _mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+        // printf("x:%d; y:%d; z:%d;\n\r",ax,ay,az);
+
+        x = x + ax; // Ｘ軸を読み込む
+        y = y + ay; // Ｙ軸を読み込む
+        z = z + az; // Ｙ軸を読み込む
+    }
+
+    double avX = x / repeatCount;
+    double avY = y / repeatCount;
+
+    int verticalAngle = atan2(avX - 507, avY - 520) / 3.14159 * 180.0;
+
+    return verticalAngle;
+}
+
+bool TI_MPU6050::isHorizontal()
+{
+    int verticalAngle = getVerticalAngle();
+
+    int baseAngle = 90;
+    bool isHorizontal;
+
+    if (verticalAngle < (baseAngle - 20)) {
+        // printf("vertical Angle : %d;\n\r", verticalAngle);
+
+        isHorizontal = false;
+    } else if ((baseAngle + 20) < verticalAngle) {
+        // printf("vertical Angle : %d;\n\r", verticalAngle);
+
+        isHorizontal = false;
+    } else {
+        isHorizontal = true;
+    }
+
+    return isHorizontal;
+}
+
+void TI_MPU6050::setSleepEnabled(bool enabled)
+{
+    _mpu.setSleepEnabled(enabled);
+}