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?
User | Revision | Line number | New 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 | } |