Evan Brown / Mbed 2 deprecated APpart3_E_start_2

completed code

Dependencies:   mbed

main.cpp@4:4b6f0c4cd39f, 2018-11-19 (annotated)

Committer:
tlee6414
Date:
Mon Nov 19 03:56:17 2018 +0000
Revision:
4:4b6f0c4cd39f
Parent:
3:461a9012682d
Child:
5:4af75af374cc
thank god

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 4:4b6f0c4cd39f 18 struct vector gyro_previous = {0, 0, 0};
tlee6414 4:4b6f0c4cd39f 19 struct vector gyro = {0, 0, 0};
tlee6414 4:4b6f0c4cd39f 20 struct vector gyro_avg;
tlee6414 4:4b6f0c4cd39f 21 struct vector xUnit = {1, 0, 0};
tlee6414 4:4b6f0c4cd39f 22 struct vector yUnit = {0, 1, 0};
tlee6414 4:4b6f0c4cd39f 23 struct vector zUnit = {0, 0, 1};
tlee6414 4:4b6f0c4cd39f 24 struct quaternion xRot;
tlee6414 4:4b6f0c4cd39f 25 struct quaternion yRot;
tlee6414 4:4b6f0c4cd39f 26 struct quaternion zRot;
tlee6414 4:4b6f0c4cd39f 27 struct quaternion finalRot;
tlee6414 4:4b6f0c4cd39f 28 float time_current = 0;
tlee6414 4:4b6f0c4cd39f 29 float time_previous = 0;
tlee6414 4:4b6f0c4cd39f 30 float delta_time = 0;
tlee6414 4:4b6f0c4cd39f 31 float radFactor = (3.14159265358979323846/180)/15.3;
tlee6414 4:4b6f0c4cd39f 32 struct vector gAngularV = {0, 0, 0};
tlee6414 4:4b6f0c4cd39f 33
tlee6414 2:a4d5e7f96e87 34
evenbrownie 0:f43994f44684 35 while(1) {
tlee6414 2:a4d5e7f96e87 36 if (!mpu.data_ready()) {
tlee6414 2:a4d5e7f96e87 37 continue;
tlee6414 2:a4d5e7f96e87 38 }
tlee6414 2:a4d5e7f96e87 39
evenbrownie 3:461a9012682d 40 //raw data
evenbrownie 3:461a9012682d 41 struct vector accel;
tlee6414 4:4b6f0c4cd39f 42
tlee6414 4:4b6f0c4cd39f 43
evenbrownie 3:461a9012682d 44
evenbrownie 3:461a9012682d 45 //bias
evenbrownie 3:461a9012682d 46 struct vector gyro_bias = {0, 0, 0};
evenbrownie 3:461a9012682d 47 struct vector accel_bias = {500, 350, 2000};
evenbrownie 3:461a9012682d 48
evenbrownie 3:461a9012682d 49 //gets raw data
evenbrownie 3:461a9012682d 50 mpu.read_raw(&gyro.x, &gyro.y, &gyro.z, &accel.x, &accel.y, &accel.z);
evenbrownie 3:461a9012682d 51 //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 52 //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
evenbrownie 3:461a9012682d 53 //pc.printf("%f %f %f\r\n", gyro.x, gyro.y, gyro.z);
evenbrownie 3:461a9012682d 54
tlee6414 4:4b6f0c4cd39f 55 //calculate angle
tlee6414 4:4b6f0c4cd39f 56 time_current = (float(millis()) / 1000);
tlee6414 4:4b6f0c4cd39f 57 delta_time = time_current - time_previous;
tlee6414 4:4b6f0c4cd39f 58 vector_add(&gyro, &gyro_previous, &gyro_avg);
tlee6414 4:4b6f0c4cd39f 59 vector_multiply(&gyro_avg, 0.5, &gAngularV);
tlee6414 4:4b6f0c4cd39f 60 vector_multiply(&gAngularV, delta_time, &gAngularV);
tlee6414 4:4b6f0c4cd39f 61 gyro_previous = gyro;
tlee6414 4:4b6f0c4cd39f 62 time_previous = time_current;
tlee6414 4:4b6f0c4cd39f 63
tlee6414 4:4b6f0c4cd39f 64 //rotate orientation vector
tlee6414 4:4b6f0c4cd39f 65 gAngularV.x *= radFactor;
tlee6414 4:4b6f0c4cd39f 66 gAngularV.y *= radFactor;
tlee6414 4:4b6f0c4cd39f 67 gAngularV.z *= radFactor;
tlee6414 4:4b6f0c4cd39f 68 quaternion_create(&xUnit, gAngularV.x, &xRot);
tlee6414 4:4b6f0c4cd39f 69 quaternion_create(&yUnit, gAngularV.y, &yRot);
tlee6414 4:4b6f0c4cd39f 70 quaternion_create(&zUnit, gAngularV.z, &zRot);
tlee6414 4:4b6f0c4cd39f 71 quaternion_multiply(&xRot, &yRot, &finalRot);
tlee6414 4:4b6f0c4cd39f 72 quaternion_multiply(&finalRot, &zRot, &finalRot);
tlee6414 4:4b6f0c4cd39f 73 quaternion_rotate(&orientation, &finalRot, &orientation);
tlee6414 4:4b6f0c4cd39f 74 vector_normalize(&orientation, &orientation);
tlee6414 2:a4d5e7f96e87 75
evenbrownie 3:461a9012682d 76 //adds bias
evenbrownie 3:461a9012682d 77 vector_add(&accel, &accel_bias, &accel);
evenbrownie 3:461a9012682d 78 vector_add(&gyro, &gyro_bias, &gyro);
evenbrownie 3:461a9012682d 79 //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
tlee6414 4:4b6f0c4cd39f 80 //pc.printf("%f %f %f\r\n", gyro.x, gyro.y, gyro.z);
tlee6414 4:4b6f0c4cd39f 81 wait(0.1);
tlee6414 4:4b6f0c4cd39f 82
evenbrownie 3:461a9012682d 83 //normalize raw data to get vector
evenbrownie 3:461a9012682d 84 struct vector accel_norm = {0, 0, 0};
evenbrownie 3:461a9012682d 85 vector_normalize(&accel, &accel_norm);
tlee6414 4:4b6f0c4cd39f 86 pc.printf("%f %f %f\r\n", orientation.x, orientation.y, orientation.z);
evenbrownie 3:461a9012682d 87
evenbrownie 3:461a9012682d 88 //wait(1);
evenbrownie 0:f43994f44684 89 }
evenbrownie 3:461a9012682d 90 }