Diff: main.cpp

Revision:

12:c8019aca78d0

Parent:

11:d68282e48fd4

Child:

13:f5cccb8beac0

--- a/main.cpp	Wed Feb 17 12:41:10 2016 +0000
+++ b/main.cpp	Wed Feb 17 18:03:04 2016 +0000
@@ -32,9 +32,12 @@
 volatile bool FUS = false;
 bool printed = false;
 int Time;
-int timeStamp = 0;
-int timeOfStart;
-int timeNow;
+int TimeStamp = 0;
+int TimeStamp2=0;
+int TimeOfStart;
+int TimeNow;
+int sync = 0;
+bool direction = false;
 
 //string sdata = "";
 
@@ -50,6 +53,8 @@
 
 Timer t;
 
+PwmOut PWM (p21);
+
 //  RAM -------------------------------------------------------------------------------------------
 BNO055_ID_INF_TypeDef       bno055_id_inf;
 BNO055_EULER_TypeDef        euler_angles;
@@ -82,8 +87,8 @@
         pc.printf("\r\nSWITCHING TO RAW DATA!\r\n");
     }
     if ( serial->rxGetLastChar() == '*') {
-        timeOfStart = t.read_ms();        
-        timeStamp = 0;
+        TimeOfStart = t.read_ms();
+        TimeStamp = 0;
     }
 }
 
@@ -201,31 +206,88 @@
     if (c == 'y') {
         bno055_calbration();
     }
+    
+    while (sync == 0) {
+        pc.printf("HELLO/");
+        if (pc.readable()) {
+            if (pc.getc() == 72) {
+                while (sync == 0) {
+                    if (pc.readable()) {
+                        if (pc.getc() == 73) {
+                            sync = 1;
+                        }
+                    }
+                }
+            }    
+        }       
+    }
+    pc.printf("START/");
     t.start();
     imu.write_reg0(0x3d, 0x07); // Default FUS
-
+    
     while (1) {
-        while (RAW == true) {            
+        while (RAW == true) {
             Time = t.read_ms();
-            timeNow = Time + timeOfStart;
-            if (timeNow > timeStamp + 9) {
+            if (Time > TimeStamp + 5) {
                 pc.printf("RAW,xLSB:,%d,xMSB:,%d,yLSB:%d,yMSB:,%d,zLSB:,%d,zMSB:,%d,"
                           ,imu.read_reg0(0x08),imu.read_reg0(0x09)
                           ,imu.read_reg0(0x0A),imu.read_reg0(0x0B)
                           ,imu.read_reg0(0x0C),imu.read_reg0(0x0D));
-                pc.printf("Time:,%d,/",timeNow);
-                timeStamp = timeNow;
+                pc.printf("Time:,%d,/",t.read_ms());
+                TimeStamp = Time;
             }
         }
         while (RAW == false) {
             Time = t.read_ms();
-            timeNow = + Time - timeOfStart;
-            if (timeNow > timeStamp + 9) {
+            TimeNow = + Time - TimeOfStart;
+            if (TimeNow > TimeStamp + 5) {
+                TimeStamp = TimeNow;
+                //imu.get_linear_accel(&linear_acc);
+//                pc.printf("FUS,T:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , , , ,"
+//                          ,TimeNow,linear_acc.x ,linear_acc.y ,linear_acc.z);
+//
+//                imu.get_Euler_Angles(&euler_angles);
+//                pc.printf("Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
+//                          euler_angles.h, euler_angles.r, euler_angles.p);
+//
+//                imu.get_quaternion(&quaternion);
+//                pc.printf("W,%d,X,%d,Y,%d,Z,%d,",
+//                          quaternion.w, quaternion.x, quaternion.y, quaternion.z);
+//
+//                imu.get_gravity(&gravity);
+//                pc.printf("X,%+6.1f,Y,%+6.1f,Z,%+6.1f,/",
+//                          gravity.x, gravity.y, gravity.z);
+                          
                 imu.get_linear_accel(&linear_acc);
-                pc.printf("FUS,Time:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , , , , , , , , /"
-                          ,timeNow,linear_acc.x ,linear_acc.y ,linear_acc.z);
-                timeStamp = timeNow;
+                imu.get_Euler_Angles(&euler_angles);
+                imu.get_quaternion(&quaternion);
+                imu.get_gravity(&gravity);
+                pc.printf("FUS,T:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , ,Y,%+6.1f,R,%+6.1f,P,%+6.1f,W,%d,X,%d,Y,%d,Z,%d,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,/"
+                          ,TimeNow,linear_acc.x ,linear_acc.y ,linear_acc.z,
+                          euler_angles.h, euler_angles.r, euler_angles.p,
+                          quaternion.w, quaternion.x, quaternion.y, quaternion.z,
+                          gravity.x, gravity.y, gravity.z);
+                          
             }
+
+            /*if (Time > TimeStamp2 + 999) {
+                if (direction == false) {
+                    PWM.pulsewidth_us(2400);
+                    //pc.printf("DIR, UP,");
+                    //pc.printf("Time:,%d,/",t.read_ms());
+                    direction = true;
+                    TimeStamp2 = Time;
+                }
+
+                else if (direction == true) {
+                    PWM.pulsewidth_us(600);
+                    //pc.printf("DIR, DOWN,");
+                    //pc.printf("Time:,%d,/",t.read_ms());
+                    direction = false;
+                    TimeStamp2 = Time;
+                }
+            }
+            */
         }
     }
 }