Diff: main.cpp

Revision:

15:304c0fcf6575

Parent:

14:418d1703806c

diff -r 418d1703806c -r 304c0fcf6575 main.cpp
--- a/main.cpp	Thu Feb 18 00:47:42 2016 +0000
+++ b/main.cpp	Mon Feb 22 18:54:27 2016 +0000
@@ -1,4 +1,4 @@
-//-----TOBY GALA AND TOM ALLEN FIGHTING CRIME-----//
+//-----TOBY GALAL AND TOM ALLEN FIGHTING CRIME-----//
 
 /*
  * mbed Application program for the mbed Nucleo F401
@@ -38,6 +38,7 @@
 int TimeStamp = 0;
 int TimeStamp2 = 0;
 int TimeOfStart = 0;
+int TimeOfStartms = 0;
 int TimeNow;
 int sync = 1;
 bool direction = false;
@@ -49,6 +50,7 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 DigitalOut pwr_onoff(p30);
+DigitalOut oscill(p20);
 
 I2C    i2c(p28, p27); // SDA, SCL
 BNO055 imu(i2c, p29);  // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
@@ -90,7 +92,8 @@
         pc.printf("\r\nSWITCHING TO RAW DATA!\r\n");
     }
     if ( serial->rxGetLastChar() == '*') {
-        TimeOfStart = t.read_ms();
+        TimeOfStart = t.read_us();
+        TimeOfStartms = t.read_ms();
         TimeStamp = 0;
         pc.printf("RESETTING TIME");
     }
@@ -179,6 +182,7 @@
 //-------------------------------------------------------------------------------------------------
 int main()
 {
+    oscill=0;
     pc.baud(921600);
     pc.attach(&rxCallback, MODSERIAL::RxIrq);
     pc.printf("BNO055 Hello World/\r\n\r\n");
@@ -236,7 +240,9 @@
     pc.printf("START/");
     t.start();
     imu.write_reg0(0x3d, 0x07); // Default FUS
-    
+    imu.write_reg0(0x3b, 0x01);
+    imu.write_reg0(0x08, 0x03);
+    int units = imu.read_reg0(0x08);
     while (1) {
         while (RAW == true) {
             Time = t.read_ms();
@@ -252,24 +258,29 @@
         while (RAW == false) {
             //Time = t.read_ms();
             //TimeNow = + t.read_ms() - TimeOfStart;
-            if (t.read_ms() - TimeOfStart > TimeStamp + 5) {
-                
-                //TimeStart = t.read_ms() - TimeOfStart;
+            
+            //TimeStart = t.read_us() - TimeOfStart;
+            
+            if (t.read_us () - TimeOfStart > TimeStamp + 7060) {                              
+                          
+                //oscill = 1;
                           
                 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,/"
-                          ,t.read_ms() - TimeOfStart,linear_acc.x ,linear_acc.y ,linear_acc.z,
+                          ,t.read_ms() - TimeOfStartms,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); 
                           
-                
-                //pc.printf("TimeStart:,%d,/",TimeStart);          
-                //pc.printf("Timend:,%d,/",+ t.read_ms() - TimeOfStart);
-                TimeStamp = t.read_ms()- TimeOfStart;
+                //oscill = 0;
+                //wait(1);
+                                         
+                TimeStamp = t.read_us()- TimeOfStart;
+                //pc.printf("TimeStart:,%d,/",TimeStart); 
+                //pc.printf("Timend:,%d,/",+ t.read_us() - TimeOfStart);
             }
 
             /*if (Time > TimeStamp2 + 999) {
@@ -292,36 +303,4 @@
             */
         }
     }
-}
-
-//         imu.get_Euler_Angles(&euler_angles);
-//            pc.printf("X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
-//                      euler_angles.h, euler_angles.r, euler_angles.p);
-//            imu.get_linear_accel(&linear_acc);
-//            pc.printf("Acc:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
-//                      linear_acc.x, linear_acc.y, linear_acc.z);
-//
-//            imu.get_gravity(&gravity);
-//            pc.printf("Gravity,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
-//                      gravity.x, gravity.y, gravity.z);
-//
-//            pc.printf("Time:,%d,\r\n",t.read_ms());
-//        imu.get_Euler_Angles(&euler_angles);
-//        pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
-//                   euler_angles.h, euler_angles.r, euler_angles.p);
-//        imu.get_quaternion(&quaternion);
-//        pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
-//                   quaternion.w, quaternion.x, quaternion.y, quaternion.z);
-//        imu.get_linear_accel(&linear_acc);
-//        pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
-//                   linear_acc.x, linear_acc.y, linear_acc.z);
-//        imu.get_gravity(&gravity);
-//        pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
-//                   gravity.x, gravity.y, gravity.z);
-//        imu.get_chip_temperature(&chip_temp);
-//        pc.printf("[T],%+d,%+d,",
-//                   chip_temp.acc_chip, chip_temp.gyr_chip);
-//        pc.printf("[S],0x%x,[M],%d\r\n",
-//                   imu.read_calib_status(), t.read_ms());
-//        pc.printf("Words\r\n");
-
+}
\ No newline at end of file