Diff: main.cpp

Revision:

13:f5cccb8beac0

Parent:

12:c8019aca78d0

Child:

14:418d1703806c

--- a/main.cpp	Wed Feb 17 18:03:04 2016 +0000
+++ b/main.cpp	Thu Feb 18 00:41:21 2016 +0000
@@ -32,11 +32,12 @@
 volatile bool FUS = false;
 bool printed = false;
 int Time;
+int TimeStart;
 int TimeStamp = 0;
-int TimeStamp2=0;
-int TimeOfStart;
+int TimeStamp2 = 0;
+int TimeOfStart = 0;
 int TimeNow;
-int sync = 0;
+int sync = 1;
 bool direction = false;
 
 //string sdata = "";
@@ -89,6 +90,7 @@
     if ( serial->rxGetLastChar() == '*') {
         TimeOfStart = t.read_ms();
         TimeStamp = 0;
+        pc.printf("RESETTING TIME");
     }
 }
 
@@ -175,38 +177,46 @@
 //-------------------------------------------------------------------------------------------------
 int main()
 {
-    pc.baud(115200);
+    pc.baud(921600);
     pc.attach(&rxCallback, MODSERIAL::RxIrq);
-    pc.printf("BNO055 Hello World\r\n\r\n");
+    pc.printf("BNO055 Hello World/\r\n\r\n");
     imu.set_mounting_position(MT_P6);
     pwr_onoff = 0;
-    pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n");
+    pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!/\r\n");
     char c = pc.getc();
     pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
     // Is BNO055 avairable?
     if (imu.chip_ready() == 0) {
         do {
-            pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
+            pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset/\r\n");
             pwr_onoff = 1;  // Power off
             wait(0.1);
             pwr_onoff = 0;  // Power on
             wait(0.02);
         } while(imu.reset());
     }
-    pc.printf("Bosch BNO055 is available now!!\r\n");
-    pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n",
+    pc.printf("Bosch BNO055 is available now!!/\r\n");
+    pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x/\r\n",
               imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));
     imu.read_id_inf(&bno055_id_inf);
-    pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
+    pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x,/",
               bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
-    pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
+    pc.printf("SW REV:0x%04x, BL REV:0x%02x/\r\n",
               bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
-    pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)\r\n");
+    pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)/\r\n");
     c = pc.getc();
     if (c == 'y') {
         bno055_calbration();
     }
     
+    pc.printf("If you would like to Synchronize the System, please hit 'y' (No: any other key)\r\n/");
+    c = pc.getc();
+    if (c == 'y') {
+        sync=0;
+        pc.printf("Synchronise/");
+        wait(0.5);
+    }    
+    
     while (sync == 0) {
         pc.printf("HELLO/");
         if (pc.readable()) {
@@ -238,36 +248,26 @@
             }
         }
         while (RAW == false) {
-            Time = t.read_ms();
-            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);
+            //Time = t.read_ms();
+            //TimeNow = + t.read_ms() - TimeOfStart;
+            if (t.read_ms() - TimeOfStart > TimeStamp + 5) {
+                
+                //TimeStart = t.read_ms() - TimeOfStart;
                           
                 imu.get_linear_accel(&linear_acc);
                 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,
+                          ,t.read_ms() - TimeOfStart,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);
+                          gravity.x, gravity.y, gravity.z); 
                           
+                
+                //pc.printf("TimeStart:,%d,/",TimeStart);          
+                //pc.printf("Timend:,%d,/",+ t.read_ms() - TimeOfStart);
+                TimeStamp = t.read_ms()- TimeOfStart;
             }
 
             /*if (Time > TimeStamp2 + 999) {