Huy pham / Mbed 2 deprecated 3DTracking

3D Tracking

Dependencies:   HMC5883L MPU6050 mbed

main.cpp@0:ee11a5d3da00, 2017-06-01 (annotated)

Committer:
huydnfly
Date:
Thu Jun 01 04:22:03 2017 +0000
Revision:
0:ee11a5d3da00
3D;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
huydnfly 0:ee11a5d3da00 1 #include "mbed.h"
huydnfly 0:ee11a5d3da00 2 #include "HMC5883L.h"
huydnfly 0:ee11a5d3da00 3 #include "MPU6050.h"
huydnfly 0:ee11a5d3da00 4 #include <math.h>
huydnfly 0:ee11a5d3da00 5
huydnfly 0:ee11a5d3da00 6 #define SDA D14
huydnfly 0:ee11a5d3da00 7 #define SCL D15
huydnfly 0:ee11a5d3da00 8 #define PI 3.14159265
huydnfly 0:ee11a5d3da00 9
huydnfly 0:ee11a5d3da00 10 #define WAIT_COMMAND 1
huydnfly 0:ee11a5d3da00 11 #define WAIT_SETUP 2
huydnfly 0:ee11a5d3da00 12 #define SETUP 3
huydnfly 0:ee11a5d3da00 13 #define SAMPLING 4
huydnfly 0:ee11a5d3da00 14
huydnfly 0:ee11a5d3da00 15 DigitalOut myled(LED1);
huydnfly 0:ee11a5d3da00 16 Serial pc(D1, D0);
huydnfly 0:ee11a5d3da00 17 float x, y, z, heading;
huydnfly 0:ee11a5d3da00 18 int16_t raw[3];
huydnfly 0:ee11a5d3da00 19 float acc[3];
huydnfly 0:ee11a5d3da00 20 float gry[3];
huydnfly 0:ee11a5d3da00 21 float angel[3];
huydnfly 0:ee11a5d3da00 22 char buffer[128];
huydnfly 0:ee11a5d3da00 23 char command;
huydnfly 0:ee11a5d3da00 24 int state;
huydnfly 0:ee11a5d3da00 25
huydnfly 0:ee11a5d3da00 26 int main() {
huydnfly 0:ee11a5d3da00 27 pc.printf("Inicializing...\r\n");
huydnfly 0:ee11a5d3da00 28 //HMC5883L hmc5883l(SDA, SCL);
huydnfly 0:ee11a5d3da00 29 MPU6050 mpu6050(SDA, SCL);
huydnfly 0:ee11a5d3da00 30 //mpu6050.calibrate(accelBias,gyroBias);
huydnfly 0:ee11a5d3da00 31 state = WAIT_COMMAND;
huydnfly 0:ee11a5d3da00 32 pc.printf("OK...\r\n");
huydnfly 0:ee11a5d3da00 33
huydnfly 0:ee11a5d3da00 34 wait(1);
huydnfly 0:ee11a5d3da00 35 while(1) {
huydnfly 0:ee11a5d3da00 36 switch(state) {
huydnfly 0:ee11a5d3da00 37 case WAIT_COMMAND: {
huydnfly 0:ee11a5d3da00 38 command = pc.getc();
huydnfly 0:ee11a5d3da00 39 if (command == 's')
huydnfly 0:ee11a5d3da00 40 state = SETUP;
huydnfly 0:ee11a5d3da00 41 else if (command == 'd')
huydnfly 0:ee11a5d3da00 42 state = SAMPLING;
huydnfly 0:ee11a5d3da00 43
huydnfly 0:ee11a5d3da00 44 } break;
huydnfly 0:ee11a5d3da00 45 case WAIT_SETUP: {
huydnfly 0:ee11a5d3da00 46 command = pc.getc();
huydnfly 0:ee11a5d3da00 47 if (command == 's')
huydnfly 0:ee11a5d3da00 48 state = SETUP;
huydnfly 0:ee11a5d3da00 49 } break;
huydnfly 0:ee11a5d3da00 50 case SETUP: {
huydnfly 0:ee11a5d3da00 51 //hmc5883l.getXYZ(raw);
huydnfly 0:ee11a5d3da00 52
huydnfly 0:ee11a5d3da00 53 //pc.printf("$x=%d y=%d z=%d;", raw[0], raw[1], raw[2]);
huydnfly 0:ee11a5d3da00 54 pc.printf("$Accelero: x=%d y=%d z=%d\n;", mpu6050.getAcceleroRawX(), mpu6050.getAcceleroRawY(), mpu6050.getAcceleroRawZ());
huydnfly 0:ee11a5d3da00 55 state = WAIT_COMMAND;
huydnfly 0:ee11a5d3da00 56 } break;
huydnfly 0:ee11a5d3da00 57 case SAMPLING: {
huydnfly 0:ee11a5d3da00 58 //hmc5883l.getXYZ(raw);
huydnfly 0:ee11a5d3da00 59 //pc.printf("$x=%d y=%d z=%d;", raw[0], raw[1], raw[2]);
huydnfly 0:ee11a5d3da00 60 //hmc58831.getHeadingXY()
huydnfly 0:ee11a5d3da00 61 //pc.printf("$Accelero: x=%d y=%d z=%d\n;", mpu6050.getAcceleroRawX(), mpu6050.getAcceleroRawY(), mpu6050.getAcceleroRawZ());
huydnfly 0:ee11a5d3da00 62
huydnfly 0:ee11a5d3da00 63 mpu6050.getAccelero(acc);
huydnfly 0:ee11a5d3da00 64 mpu6050.getGyro(gry);
huydnfly 0:ee11a5d3da00 65 mpu6050.getAcceleroAngle(angel);
huydnfly 0:ee11a5d3da00 66 //pc.printf("ACC: %f %f %f\n",acc[0], acc[1], acc[2]);
huydnfly 0:ee11a5d3da00 67 //pc.printf("GRY: %f %f %f\n",gry[0], gry[1], gry[2]);
huydnfly 0:ee11a5d3da00 68 pc.printf("%f %f %f %f %f %f\n",gry[0], gry[1], gry[2], acc[0], acc[1], acc[2]);
huydnfly 0:ee11a5d3da00 69 //pc.printf("%f %f %f %f %f %f\n", angel[0], angel[1], angel[2], acc[0], acc[1], acc[2]);
huydnfly 0:ee11a5d3da00 70 wait_ms(20);
huydnfly 0:ee11a5d3da00 71 //state = WAIT_COMMAND;
huydnfly 0:ee11a5d3da00 72 } break;
huydnfly 0:ee11a5d3da00 73 default: break;
huydnfly 0:ee11a5d3da00 74 }
huydnfly 0:ee11a5d3da00 75 }
huydnfly 0:ee11a5d3da00 76 }