main.cpp

00001 #include "mbed.h"
00002 #include "HMC5883L.h"
00003 #include "MPU6050.h"
00004 #include <math.h>
00005  
00006 #define SDA      D14
00007 #define SCL      D15
00008 #define PI       3.14159265
00009  
00010 #define WAIT_COMMAND   1
00011 #define WAIT_SETUP  2
00012 #define SETUP  3
00013 #define SAMPLING    4
00014 
00015 DigitalOut myled(LED1);
00016 Serial pc(D1, D0);
00017 float x, y, z, heading;
00018 int16_t raw[3];
00019 float acc[3];
00020 float gry[3];
00021 float angel[3];
00022 char buffer[128];
00023 char command;
00024 int state;
00025 
00026 int main() {
00027     pc.printf("Inicializing...\r\n");
00028     //HMC5883L hmc5883l(SDA, SCL);
00029     MPU6050 mpu6050(SDA, SCL);
00030     //mpu6050.calibrate(accelBias,gyroBias);
00031     state = WAIT_COMMAND;
00032     pc.printf("OK...\r\n");
00033     
00034     wait(1);
00035     while(1) {
00036         switch(state) {
00037         case WAIT_COMMAND: {
00038             command = pc.getc();
00039             if (command == 's') 
00040                 state = SETUP;
00041             else if (command == 'd')
00042                 state = SAMPLING;
00043                 
00044             } break;
00045         case WAIT_SETUP: {
00046             command = pc.getc();
00047             if (command == 's') 
00048                 state = SETUP;
00049             } break;
00050         case SETUP: {
00051             //hmc5883l.getXYZ(raw);
00052             
00053             //pc.printf("$x=%d y=%d z=%d;", raw[0], raw[1], raw[2]);
00054             pc.printf("$Accelero: x=%d y=%d z=%d\n;", mpu6050.getAcceleroRawX(), mpu6050.getAcceleroRawY(), mpu6050.getAcceleroRawZ());
00055             state = WAIT_COMMAND;
00056             } break;
00057         case SAMPLING: {
00058             //hmc5883l.getXYZ(raw);
00059             //pc.printf("$x=%d y=%d z=%d;", raw[0], raw[1], raw[2]);
00060             //hmc58831.getHeadingXY()
00061             //pc.printf("$Accelero: x=%d y=%d z=%d\n;", mpu6050.getAcceleroRawX(), mpu6050.getAcceleroRawY(), mpu6050.getAcceleroRawZ());
00062             
00063             mpu6050.getAccelero(acc);
00064             mpu6050.getGyro(gry);
00065             mpu6050.getAcceleroAngle(angel);
00066             //pc.printf("ACC: %f %f %f\n",acc[0], acc[1], acc[2]);
00067             //pc.printf("GRY: %f %f %f\n",gry[0], gry[1], gry[2]);
00068             pc.printf("%f %f %f %f %f %f\n",gry[0], gry[1], gry[2], acc[0], acc[1], acc[2]);
00069             //pc.printf("%f %f %f %f %f %f\n", angel[0], angel[1], angel[2], acc[0], acc[1], acc[2]);
00070             wait_ms(20);
00071             //state = WAIT_COMMAND;
00072             } break;
00073         default: break;
00074         }
00075     }
00076 }