Diff: main.cpp

Revision:

0:d1cab7bc0553

Child:

3:a5799a7d3bd1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 17 06:34:27 2016 +0000
@@ -0,0 +1,32 @@
+// NuMaker-PFM-NUC472 : I2C0 reading IMU to calculate tilt angle
+#include "mbed.h"
+#include "math.h"
+#include "mpu6500.h"
+
+#define PI 3.14159265359
+
+I2C i2c0(PC_9, PA_15); // I2C0_SDA, I2C0_SCL
+
+MPU6500 IMU; // IMU use on-board MPU6500
+
+int main() {
+    int16_t accX,  accY,  accZ;
+    float X2, Y2, Z2;
+    float theta, psi, phi;
+    
+    i2c0.frequency(400000);    
+    IMU.initialize();
+    
+    while(true) {
+       accX = IMU.getAccelXvalue();
+       accY = IMU.getAccelYvalue();
+       accZ = IMU.getAccelZvalue();
+       printf("Acc: %6d, %6d, %6d, ",   accX, accY, accZ);
+       // calculate tilt (degree = radians *180 / PI)
+       X2 = pow(accX,2.0); Y2 = pow(accY, 2.0); Z2 = pow(accZ,2.0);
+       theta = atan(accX / sqrt(Y2 + Z2)) *180 /PI;
+       psi   = atan(accY / sqrt(Z2 + X2)) *180 /PI;
+       phi   = atan(sqrt(X2 + Y2) / accZ) *180 /PI;
+       printf("theta=%d, psi=%d, phi=%d\n\r", (int) theta, (int) psi, (int) phi);              
+    }
+}