Diff: main.cpp

Revision:

3:123b546e4a5c

Parent:

2:0cccf85f9b3f

Child:

4:5ab2bb2f062b

--- a/main.cpp	Mon Apr 20 19:13:34 2015 +0000
+++ b/main.cpp	Tue Jun 30 16:06:47 2015 +0000
@@ -16,55 +16,150 @@
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
+/* This project was created by Elecia White of Embedded.fm and Logical Elegance using hardware
+*  supplied by element14. It uses 
+*    * a FRDM-KL05Z board (http://www.newark.com/webapp/wcs/stores/servlet/ProductDisplay?catalogId=15003&langId=-1&urlRequestType=Base&partNumber=53W2646&storeId=10194)
+*    * an IMU sensor toolbox shield (http://www.newark.com/webapp/wcs/stores/servlet/ProductDisplay?catalogId=15003&langId=-1&urlRequestType=Base&partNumber=30Y6365&storeId=10194)
+*    * a momentary button connected to A5 and ground
+* Together, these create a sort of "hello world" for the accelerometer, gyro, and magnetometer so 
+* people can see what each sensor does.
+*
+* On boot, the system will have no LED running though it will be outputting IMU
+* data on the serial port available from USB (115200, 8, None, 1).
+*
+* Press the button to go into accelerometer demo mode. The LED will come on and 
+* tell you upon which axis it feels gravity (red = X, green = Y, blue = Z). 
+* As you move it, the LED will change. If you jiggle it, you can see the light 
+* change in intensity. A gentle toss up might lead to a dark sensor at the top 
+* of the arc (free fall!). And if you tap on the device, you will likely see a 
+* brighter flare of the LED: accelerometers do really well with tap detection.
+* 
+* Press the button again to get into gyro demo mode. The LED will go off until 
+* you turn the unit. Spinning it around one axis is the obvious mechanism. But 
+* try swinging it back and forth in different direction. Look at the joints on 
+* your arm, what colors do you see when you move (and how consistently)? 
+* 
+* Press the button again to go into the North Star mode. Remember, it might light 
+* your way home, but only if you and it are pointed in the same direction.
+*
+* For more information, see the element14 post.
+*
+*/
 #include "mbed.h"
 #include "FXOS8700Q.h"
 #include "FXAS21000.h"
+#include "led.h"
+#include "Adafruit_9DOF.h"
+#include "northStarFuncs.h"
+#include "button.h"
 
+#define ACCEL_FLOAT_TO_MG_INT (1000.0F) // G to mG
+#define PI_F 3.14159265
+#define DEGREES_TO_MRADS       (1000.0*(2.0*PI_F/360.0))
 
 FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
 FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
 FXAS21000 gyro(D14, D15);
 
-DigitalOut ledpin(LED1);
+Serial pc(USBTX, USBRX);
+SMART_LED  smartLed(LED1, LED2, LED3);
+DigitalIn  gButton(A5, PullUp);
+
+typedef struct {
+    int16_t a[NUM_AXIS];         //  3D accelerometer data in 0.001 G (milliGs)
+    int16_t m[NUM_AXIS];            //  3D magnetometer data in 0.001 gauss (milliGauss)
+    int16_t g[NUM_AXIS];            //  3D angular rate darta in degrees per second
+} tImuData;
+
+
+// UpdateLeds uses the inertial data to set the LED (according to the current mode).
+// It alsu updates the current mode when the button is pressed
+int UpdateLed(tImuData *imuData)
+{
+  static eModes gMode = OFF;
+  uint8_t r = 0, b = 0, g = 0;
+  int delayMs = 10;
 
-Serial pc(USBTX, USBRX);
+  switch (gMode) {
+  case ACCEL:
+    // the color brightness is the abs value of the direction,
+    // scaled to one G
+    r = conditionAccels(imuData->a[X]);
+    g = conditionAccels(imuData->a[Y]);
+    b = conditionAccels(imuData->a[Z]);
+    delayMs = 10; // 100 Hz readings on the accel
+    break;
+  case MAG:
+     tOrientation orientation;
+     if (fusionGetOrientation(imuData->a, imuData->m, &orientation)) {
+        // N = white, E = green, W = red, S = dark
+        r = conditionMags(orientation.heading, -45.0, 120.0);
+        g = conditionMags(orientation.heading, 45.0, 120.0);
+        b = conditionMags(orientation.heading, 0.0, 120.0);        
+    } else{
+        printf("orientation failed\r\n");
+    }
+    delayMs = 20; // 50 Hz readings on the mag
+    break;
+  case GYRO:
+    // scale the brightness to the motion being 
+    // experienced, where X = R, Y = B, Z = G
+    r = conditionGyros(imuData->g[X]);
+    g = conditionGyros(imuData->g[Y]);
+    b = conditionGyros(imuData->g[Z]);
+    delayMs = 10;
+    break;
+  default:
+    r = g = b = 0;
+    delayMs = 10;
+    break;
+  }
+  if (debounceButtonPress(gButton)) { 
+     printf("Button pressed from %d", gMode);
+     gMode = changeMode(gMode);
+     printf(" to new mode %d\r\n", gMode);
+  }  
+  smartLed.set(r, g, b);
+  return (delayMs);
+}
 
 int main()
-{
+{        
     pc.baud(115200);    
     
     float gyro_data[3];
     MotionSensorDataUnits adata;
     MotionSensorDataUnits mdata;
-    //int16_t acc_raw[3];
-
     printf("\r\nStarting\r\n\r\n");
 
     combo_acc.enable();
     combo_mag.enable();
     printf("FXOS8700 Combo mag = %X\r\n", combo_mag.whoAmI());
     printf("FXOS8700 Combo acc = %X\r\n", combo_acc.whoAmI());
-
     printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
-    
-    wait(3);
-    
+        
     while(1) {
-        ledpin = 0;
+        tImuData imuData;
         combo_acc.getAxis(adata);
+        imuData.a[X] = adata.x * ACCEL_FLOAT_TO_MG_INT;
+        imuData.a[Y] = adata.y * ACCEL_FLOAT_TO_MG_INT;
+        imuData.a[Z] = adata.z * ACCEL_FLOAT_TO_MG_INT;
         printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
         
-        combo_mag.getAxis(mdata);
+        combo_mag.getAxis(mdata); // data in mGauss already
+        imuData.m[X] = mdata.x;
+        imuData.m[Y] = mdata.y;
+        imuData.m[Z] = mdata.z;
         printf("FXOS8700 Mag:   X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
-
+        
         gyro.ReadXYZ(gyro_data);
-        printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
-        
-       
+        imuData.g[X] = gyro_data[0] * DEGREES_TO_MRADS;
+        imuData.g[Y] = gyro_data[1] * DEGREES_TO_MRADS;
+        imuData.g[Z] = gyro_data[2] * DEGREES_TO_MRADS;
+        printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);        
         printf("\r\n");
         
-        
-        ledpin = 1;
-        wait(1);
+        int delayMs = UpdateLed(&imuData);
+        wait_ms(delayMs); 
     }
 }