Evan Brown / Mbed 2 deprecated APpart3_E_start

11/18

Dependencies:   mbed

main.cpp@3:461a9012682d, 2018-11-19 (annotated)

Committer:
evenbrownie
Date:
Mon Nov 19 01:28:14 2018 +0000
Revision:
3:461a9012682d
Parent:
2:a4d5e7f96e87 
part E

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tlee6414 2:a4d5e7f96e87 1 #include "sensor_fusion.h"
evenbrownie 3:461a9012682d 2 #include "quaternion.h"
evenbrownie 3:461a9012682d 3 #include "millis.h"
evenbrownie 3:461a9012682d 4 #include "mbed.h"
evenbrownie 3:461a9012682d 5
evenbrownie 0:f43994f44684 6
tlee6414 2:a4d5e7f96e87 7 MPU6050 mpu(SDA,SCL);
evenbrownie 0:f43994f44684 8
evenbrownie 3:461a9012682d 9 Serial pc(USBTX,USBRX,115200);
evenbrownie 3:461a9012682d 10
evenbrownie 3:461a9012682d 11
evenbrownie 3:461a9012682d 12 void normalize(struct vector *raw, struct vector *normalized);
evenbrownie 3:461a9012682d 13
evenbrownie 0:f43994f44684 14 int main() {
tlee6414 2:a4d5e7f96e87 15 mpu.start();
evenbrownie 3:461a9012682d 16 millis_begin();
evenbrownie 3:461a9012682d 17 struct vector orientation = {0, 0, 1}; //starting point
tlee6414 2:a4d5e7f96e87 18
evenbrownie 0:f43994f44684 19 while(1) {
tlee6414 2:a4d5e7f96e87 20 if (!mpu.data_ready()) {
tlee6414 2:a4d5e7f96e87 21 continue;
tlee6414 2:a4d5e7f96e87 22 }
tlee6414 2:a4d5e7f96e87 23
evenbrownie 3:461a9012682d 24 //raw data
evenbrownie 3:461a9012682d 25 struct vector accel;
evenbrownie 3:461a9012682d 26 struct vector gyro;
evenbrownie 3:461a9012682d 27
evenbrownie 3:461a9012682d 28 //bias
evenbrownie 3:461a9012682d 29 struct vector gyro_bias = {0, 0, 0};
evenbrownie 3:461a9012682d 30 struct vector accel_bias = {500, 350, 2000};
evenbrownie 3:461a9012682d 31
evenbrownie 3:461a9012682d 32 //gets raw data
evenbrownie 3:461a9012682d 33 double time = millis();
evenbrownie 3:461a9012682d 34 mpu.read_raw(&gyro.x, &gyro.y, &gyro.z, &accel.x, &accel.y, &accel.z);
evenbrownie 3:461a9012682d 35 double delta_time = millis() - time; //gets time taken to turn
evenbrownie 3:461a9012682d 36 //pc.printf("Accel: (%f, %f, %f)\n Gyro: (%f, %f, %f)\r\n", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
evenbrownie 3:461a9012682d 37 //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
evenbrownie 3:461a9012682d 38 //pc.printf("%f %f %f\r\n", gyro.x, gyro.y, gyro.z);
evenbrownie 3:461a9012682d 39
tlee6414 2:a4d5e7f96e87 40
evenbrownie 3:461a9012682d 41 //adds bias
evenbrownie 3:461a9012682d 42 vector_add(&accel, &accel_bias, &accel);
evenbrownie 3:461a9012682d 43 vector_add(&gyro, &gyro_bias, &gyro);
evenbrownie 3:461a9012682d 44 //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
evenbrownie 3:461a9012682d 45 pc.printf("%f %f %f\r\n", gyro.x, gyro.y, gyro.z);
evenbrownie 3:461a9012682d 46 //wait(0.1);
evenbrownie 3:461a9012682d 47
evenbrownie 3:461a9012682d 48 //normalize raw data to get vector
evenbrownie 3:461a9012682d 49 struct vector accel_norm = {0, 0, 0};
evenbrownie 3:461a9012682d 50 vector_normalize(&accel, &accel_norm);
evenbrownie 3:461a9012682d 51 //pc.printf("%f %f %f\r\n", accel_norm.x, accel_norm.y, accel_norm.z);
evenbrownie 3:461a9012682d 52 wait(0.1);
evenbrownie 3:461a9012682d 53
evenbrownie 3:461a9012682d 54 //create a quaternion
evenbrownie 3:461a9012682d 55 struct vector gyro_norm;
evenbrownie 3:461a9012682d 56 float angle = vector_normalize(&gyro, &gyro_norm);
evenbrownie 3:461a9012682d 57
evenbrownie 3:461a9012682d 58 struct vector orientation = {0, 0, 1};
evenbrownie 3:461a9012682d 59 quaternion quat;
evenbrownie 3:461a9012682d 60 quaternion_create(&gyro_norm, angle, &quat);
evenbrownie 3:461a9012682d 61
evenbrownie 3:461a9012682d 62 //wait(1);
evenbrownie 0:f43994f44684 63 }
evenbrownie 3:461a9012682d 64 }