Diff: main.cpp

Revision:

0:ee11a5d3da00

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jun 01 04:22:03 2017 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#include "HMC5883L.h"
+#include "MPU6050.h"
+#include <math.h>
+ 
+#define SDA      D14
+#define SCL      D15
+#define PI       3.14159265
+ 
+#define WAIT_COMMAND   1
+#define WAIT_SETUP  2
+#define SETUP  3
+#define SAMPLING    4
+
+DigitalOut myled(LED1);
+Serial pc(D1, D0);
+float x, y, z, heading;
+int16_t raw[3];
+float acc[3];
+float gry[3];
+float angel[3];
+char buffer[128];
+char command;
+int state;
+
+int main() {
+    pc.printf("Inicializing...\r\n");
+    //HMC5883L hmc5883l(SDA, SCL);
+    MPU6050 mpu6050(SDA, SCL);
+    //mpu6050.calibrate(accelBias,gyroBias);
+    state = WAIT_COMMAND;
+    pc.printf("OK...\r\n");
+    
+    wait(1);
+    while(1) {
+        switch(state) {
+        case WAIT_COMMAND: {
+            command = pc.getc();
+            if (command == 's') 
+                state = SETUP;
+            else if (command == 'd')
+                state = SAMPLING;
+                
+            } break;
+        case WAIT_SETUP: {
+            command = pc.getc();
+            if (command == 's') 
+                state = SETUP;
+            } break;
+        case SETUP: {
+            //hmc5883l.getXYZ(raw);
+            
+            //pc.printf("$x=%d y=%d z=%d;", raw[0], raw[1], raw[2]);
+            pc.printf("$Accelero: x=%d y=%d z=%d\n;", mpu6050.getAcceleroRawX(), mpu6050.getAcceleroRawY(), mpu6050.getAcceleroRawZ());
+            state = WAIT_COMMAND;
+            } break;
+        case SAMPLING: {
+            //hmc5883l.getXYZ(raw);
+            //pc.printf("$x=%d y=%d z=%d;", raw[0], raw[1], raw[2]);
+            //hmc58831.getHeadingXY()
+            //pc.printf("$Accelero: x=%d y=%d z=%d\n;", mpu6050.getAcceleroRawX(), mpu6050.getAcceleroRawY(), mpu6050.getAcceleroRawZ());
+            
+            mpu6050.getAccelero(acc);
+            mpu6050.getGyro(gry);
+            mpu6050.getAcceleroAngle(angel);
+            //pc.printf("ACC: %f %f %f\n",acc[0], acc[1], acc[2]);
+            //pc.printf("GRY: %f %f %f\n",gry[0], gry[1], gry[2]);
+            pc.printf("%f %f %f %f %f %f\n",gry[0], gry[1], gry[2], acc[0], acc[1], acc[2]);
+            //pc.printf("%f %f %f %f %f %f\n", angel[0], angel[1], angel[2], acc[0], acc[1], acc[2]);
+            wait_ms(20);
+            //state = WAIT_COMMAND;
+            } break;
+        default: break;
+        }
+    }
+}