3D Tracking
Dependencies: HMC5883L MPU6050 mbed
Diff: main.cpp
- Revision:
- 0:ee11a5d3da00
diff -r 000000000000 -r ee11a5d3da00 main.cpp --- /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; + } + } +}