Diff: bom_acc_341.cpp

Revision:

4:f490860cf06d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bom_acc_341.cpp	Mon Oct 06 00:36:33 2014 +0000
@@ -0,0 +1,145 @@
+#include "mbed.h"
+#include "MMA8451Q.h"
+#include "SLCD.h"
+
+/* 
+Test of the accelerometer, digital I/O, on-board LCD screen.
+ Looing at vector product of the x-y components of the accelerometer.
+ Works pretty well. Still rough, program wise - sc 140710
+ */
+
+#define DATATIME 0.150
+#define CHANGELIMIT .800
+#define LEDON false
+#define LEDOFF true
+#define PROGNAME "ACCLCD341bom"
+#define PRINTDBUG
+// 
+#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
+  PinName const SDA = PTE25;  // Data pins for the accelerometer/magnetometer.
+  PinName const SCL = PTE24;  // DO NOT CHANGE
+#elif defined (TARGET_KL05Z)
+  PinName const SDA = PTB4;
+  PinName const SCL = PTB3;
+#else
+  #error TARGET NOT DEFINED
+#endif
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+enum vectorStates {MIDDLESTATE, LANDSCAPESTATE, HORIZONTALSTATE}; 
+
+SLCD slcd; //define LCD display
+
+MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+Serial pc(USBTX, USBRX);
+
+float sqrt_newt(float argument) {
+    float xsqr = argument;
+    float xnew = 0.0;
+    float epsilon = 10e-7;
+    int intmax = 20;
+    float fCube = 2.0;
+    float xold = xsqr/fCube;
+    float delta = 1;
+    int cube = 2;
+    float a = 0.333333;    
+    int i = 0;
+    
+    while(delta > epsilon || i > intmax)
+    {
+        xnew = a*(pow(xsqr/xold, cube) + (fCube*xold));
+        delta = abs(xnew-xold);
+        xold = xnew;
+        i++;
+    }    
+    return (xold);
+}
+   
+
+void LCDMess(char *lMess, float dWait){
+        slcd.Home();
+        slcd.clear();
+        slcd.printf(lMess);
+        wait(dWait);
+} 
+
+
+int main() {
+    float xAcc;
+    float yAcc;
+    float vector;
+    DigitalOut greenColor(LED_GREEN);
+    DigitalOut redColor(LED_RED);
+    vectorStates PGMState = MIDDLESTATE;
+    int ledState = LEDON;
+    //Timer LEDTimer;
+    char lcdData[10]; //buffer needs places for decimal pt and colon
+    //LEDTimer.start();
+    //LEDTimer.reset();
+    
+#ifdef PRINTDBUG
+        pc.printf(PROGNAME);
+#endif
+// main loop forever 
+    while(true) {
+
+//Get accelerometer data - tilt angles minus offset for zero mark.
+        xAcc = abs(acc.getAccX());
+        yAcc = abs(acc.getAccY());     
+ // Calulate vector sum of x and y reading.       
+        vector = sqrt_newt(pow(xAcc,2) + pow(yAcc,2));
+        
+/*************************** STATE MACHINE ***********************/
+        switch(PGMState)
+        {
+            case MIDDLESTATE:
+                if(xAcc < CHANGELIMIT && yAcc < CHANGELIMIT)
+                {
+                    redColor.write(ledState);
+                    greenColor.write(ledState);
+                }
+                else if(xAcc > CHANGELIMIT)
+                {
+                    PGMState = LANDSCAPESTATE;
+                    
+                }
+                else if(yAcc > CHANGELIMIT)
+                {
+                    PGMState = HORIZONTALSTATE;
+                }                
+            break;
+            case LANDSCAPESTATE:
+                if(xAcc > CHANGELIMIT)
+                {
+                    redColor.write(ledState);
+                    greenColor.write(!ledState);
+                }
+                else
+                    PGMState = MIDDLESTATE;
+            break;
+            case HORIZONTALSTATE:
+            if(yAcc > CHANGELIMIT)
+            {
+                
+                redColor.write(!ledState);
+                greenColor.write(ledState);
+            }
+            else
+                PGMState = MIDDLESTATE;
+            break;
+        }
+            
+        
+ 
+#ifdef PRINTDBUG
+        pc.printf("xAcc = %f\r\n", xAcc);
+        pc.printf("yAcc = %f\r\n", yAcc);
+        pc.printf("vector = %f\r\n",  vector);
+#endif
+
+        sprintf (lcdData,"%4.3f",yAcc);
+        LCDMess(lcdData, DATATIME);
+// Wait then do the whole thing again.
+        wait(DATATIME);
+    }
+}
\ No newline at end of file