Diff: main.cpp

Revision:

5:b1c4a117eb4b

Parent:

3:82e223a4a4e4

Child:

6:4265663c9a34

--- a/main.cpp	Mon Sep 19 13:00:16 2016 +0000
+++ b/main.cpp	Fri Sep 08 19:50:34 2017 +0000
@@ -1,17 +1,21 @@
 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
 
-#define LOG_RATE 50.0
+#define STAT_RATE 1.0
+#define IMU_RATE 50.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
 #include "mbed.h"
 
 #include "BNO055.h"
+#include "NeoStrip.h"
 
 
 BNO055 imu(p9, p10);
-
+NeoStrip leds(p30,LED_CLUSTERS*LED_PER_CLUSTER);
 
 int left;
 float saturateCmd(float cmd);
@@ -25,17 +29,13 @@
 Serial xbee(p28, p27); // tx, rx for Xbee
 
 
-
-
 Timer t; // create timer instance
+float t_imu,t_stat;
 
-float t_imu,t_cmd,str_cmd,thr_cmd,str,thr;
-float t_hall, dt_hall,t_run,t_stop,t_log;
-bool armed, auto_ctrl;
-float wheel_spd;
-float arm_clock;
-bool str_cond,thr_cond,run_ctrl,log_data;
-bool log_imu,log_bno,log_odo,log_mag = false;
+void setLED(int  *colors,float brightness);
+float wrapTo2pi(float ang);
+int stat_colors[4] = {RED,ORANGE,YELLOW,GREEN};
+int colors[4];
 
 int main()
 {
@@ -43,38 +43,25 @@
     pc.baud(115200);
     xbee.baud(115200);
 
-//imu.init_MPU_i2c();
-    left = 0;
-    str_cmd = 0;
-    str=0;
-    thr=0;
-    thr_cmd = 0;
-    arm_clock =0;
-    str_cond = false;
-    thr_cond = false;
-    armed = false;
-    auto_ctrl = false;
-    run_ctrl = false;
-    log_data = false;
 
+// Initialize timers for IMU and Status Update
     t.start();
     t_imu = t.read();
-    t_cmd = 0;
+    t_stat =t.read();
 
     status_LED = 1;
 
     if(imu.check()) {
-        
+
         pc.printf("BNO055 connected\r\n");
         imu.setmode(OPERATION_MODE_CONFIG);
-        imu.SetExternalCrystal(1);
-        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
-        imu.set_angle_units(DEGREES);
+        imu.SetExternalCrystal(1);        
+        imu.set_angle_units(RADIANS);
         imu.set_accel_units(MPERSPERS);
-        imu.set_anglerate_units(DEG_PER_SEC);        
+        imu.set_anglerate_units(RAD_PER_SEC);        
         imu.setoutputformat(WINDOWS);
         imu.set_mapping(2);
-
+        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
         
             
     } else {
@@ -95,18 +82,45 @@
     pc.printf("ES456 Vehicle Sensor Logger\r\n");
     while(1){
                          
-        imu.get_angles();
-        imu.get_accel();
-        imu.get_gyro();
-        imu.get_mag();
-         if(t.read()-t_imu > (1/LOG_RATE)) {
+        
+        if(t.read()-t_stat > (1/STAT_RATE)){
+            uint8_t acc_stat;
+            uint8_t gyro_stat;
+            uint8_t mag_stat;
+            uint8_t imu_stat;
+            imu.get_calib();
+            
+           // int x = (number >> (8*n)) & 0xff
+            
+            mag_stat = (imu.calib & 0x03);
+            acc_stat = (imu.calib & 0x0C)>> 2;
+            gyro_stat = (imu.calib & 0x30) >> 4;
+            imu_stat = (imu.calib & 0xC0) >> 6;
+                        
+          //  pc.printf("%mag %d, acc %d, gyro %d imu %d\r\n",mag_stat, acc_stat, gyro_stat, imu_stat);                
+            colors[0] = stat_colors[mag_stat];
+            colors[1] = stat_colors[acc_stat];
+            colors[2] = stat_colors[gyro_stat];
+            setLED(colors,0.05);
+            t_stat = t.read();
+        }
+        
+         if(t.read()-t_imu > (1/IMU_RATE)) {
 
+            imu.get_angles();
+            imu.get_accel();
+            imu.get_gyro();
+            imu.get_mag();
 
              //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);             
+             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);
@@ -119,5 +133,37 @@
 
 } // main
 
+float wrapTo2pi(float ang){
+
+if(ang > 2*PI){
+    ang = ang - 2*PI;
+}
+
+if(ang < 0){
+    ang = ang + 2*PI;
+}
+
+return ang;
 
 
+}
+void setLED(int  *colors,float brightness)
+{
+
+    leds.setBrightness(brightness);
+
+    int cidx = 0;
+    int ctr = 0;
+    for (int i=0; i<LED_PER_CLUSTER*LED_CLUSTERS; i++) {
+
+        if(ctr >= LED_PER_CLUSTER) {
+            ctr = 0;
+            cidx++;
+        }
+        leds.setPixel(i,colors[cidx]);
+        ctr++;
+    }
+    leds.write();
+}
+
+