Diff: main.cpp

Revision:

3:461a9012682d

Parent:

2:a4d5e7f96e87

Child:

4:4b6f0c4cd39f

--- a/main.cpp	Thu Nov 15 01:36:16 2018 +0000
+++ b/main.cpp	Mon Nov 19 01:28:14 2018 +0000
@@ -1,20 +1,64 @@
 #include "sensor_fusion.h"
+#include "quaternion.h"
+#include "millis.h"
+#include "mbed.h"
+
 
 MPU6050 mpu(SDA,SCL);
 
+Serial pc(USBTX,USBRX,115200);
+
+
+void normalize(struct vector *raw, struct vector *normalized);
+
 int main() {
     mpu.start();
+    millis_begin();
+    struct vector orientation = {0, 0, 1}; //starting point
     
     while(1) {
         if (!mpu.data_ready()) {
             continue;
         }
         
-        float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;
-        mpu.read_raw(&gyro_x, &gyro_y, &gyro_z, &accel_x, &accel_y, &accel_z);
+        //raw data
+        struct vector accel;
+        struct vector gyro;
+        
+        //bias
+        struct vector gyro_bias = {0, 0, 0};
+        struct vector accel_bias = {500, 350, 2000};
+        
+        //gets raw data
+        double time = millis();
+        mpu.read_raw(&gyro.x, &gyro.y, &gyro.z, &accel.x, &accel.y, &accel.z);
+        double delta_time = millis() - time; //gets time taken to turn
+        //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);
+        //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
+        //pc.printf("%f %f %f\r\n",  gyro.x, gyro.y, gyro.z);
+        
         
-        printf("ax:%f\n ay: %f\n az: %f\n gx: %f\n gy: %f\n gz: %f\r\n", accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z);
-        printf("____________________\n");
-        wait(1);
+        //adds bias
+        vector_add(&accel, &accel_bias, &accel);
+        vector_add(&gyro, &gyro_bias, &gyro);
+        //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
+        pc.printf("%f %f %f\r\n",  gyro.x, gyro.y, gyro.z);
+        //wait(0.1);
+        
+        //normalize raw data to get vector
+        struct vector accel_norm = {0, 0, 0};
+        vector_normalize(&accel, &accel_norm);
+        //pc.printf("%f %f %f\r\n", accel_norm.x, accel_norm.y, accel_norm.z);
+        wait(0.1);
+        
+        //create a quaternion
+        struct vector gyro_norm;
+        float angle = vector_normalize(&gyro, &gyro_norm);
+        
+        struct vector orientation = {0, 0, 1};
+        quaternion quat;
+        quaternion_create(&gyro_norm, angle, &quat);
+        
+        //wait(1);
     }
-}
\ No newline at end of file
+}