Diff: main.cpp

Revision:

6:4265663c9a34

Parent:

5:b1c4a117eb4b

Child:

8:7f35f3cd8235

--- a/main.cpp	Fri Sep 08 19:50:34 2017 +0000
+++ b/main.cpp	Tue Sep 19 17:27:59 2017 +0000
@@ -1,13 +1,18 @@
 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
 
 #define STAT_RATE 1.0
-#define IMU_RATE 50.0
+#define IMU_RATE 150.0
 #define LOOP_RATE 200.0
 #define CMD_TIMEOUT 1.0
 #define GEAR_RATIO (1/2.75)
 #define PI 3.14159
 #define LED_CLUSTERS 3
 #define LED_PER_CLUSTER 1
+#define LOG_IMU 1
+#define LOG_MAG 2
+#define LOG_EUL 3
+#define LOG_ODO 4
+
 #include "mbed.h"
 
 #include "BNO055.h"
@@ -28,6 +33,7 @@
 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
 Serial xbee(p28, p27); // tx, rx for Xbee
 
+int log_it;
 
 Timer t; // create timer instance
 float t_imu,t_stat;
@@ -48,7 +54,7 @@
     t.start();
     t_imu = t.read();
     t_stat =t.read();
-
+    log_it = 0;
     status_LED = 1;
 
     if(imu.check()) {
@@ -107,20 +113,38 @@
         
          if(t.read()-t_imu > (1/IMU_RATE)) {
 
-            imu.get_angles();
-            imu.get_accel();
-            imu.get_gyro();
-            imu.get_mag();
+
+            switch (log_it){            
+                case (LOG_IMU): {
+                    imu.get_accel();  
+                    imu.get_gyro(); 
+                    pc.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);                
+                                                 
+                    break;
+                }
+                case (LOG_MAG): {
+                    imu.get_mag();   
+                    pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);                                 
+                    break;
+                }
+                case (LOG_EUL): {
+                    imu.get_angles();
+                    pc.printf("$RPY,%.3f, %.3f, %.3f\r\n", imu.euler.roll,imu.euler.pitch,wrapTo2pi(imu.euler.yaw));                                    
+                    break;
+                }
+                default :{
+                    log_it = 0;
+                }                                        
+            
+            }
+                log_it++;
+
 
              //pc.printf("$MAD,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f,%.1f,%.1f\r\n", imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.accel.x,imu.accel.y,imu.accel.z,imu.mag.x,imu.mag.y,imu.mag.z);             
              //xbee.printf("$MAD,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f,%.1f,%.1f\r\n", imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.accel.x,imu.accel.y,imu.accel.z,imu.mag.x,imu.mag.y,imu.mag.z);
 
-             pc.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z); 
-             wait(0.001);            
-             pc.printf("$RPY,%.3f, %.3f, %.3f\r\n", imu.euler.roll,imu.euler.pitch,wrapTo2pi(imu.euler.yaw));
-             wait(0.001);                                      
-             pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
-             wait(0.001);            
+ 
+    
              
            //  xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);             
            //  xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);