Revision 3:b425cdf7c99c, committed 2014-10-06

Comitter:

scohennm

Date:

Mon Oct 06 19:31:52 2014 +0000

Parent:

2:6003ed409def

Commit message:

8.1 MT solution for accelerometer program with hysteresis

Changed in this revision

diff -r 6003ed409def -r b425cdf7c99c acc_341.cpp
--- a/acc_341.cpp	Thu Sep 25 23:45:35 2014 +0000
+++ b/acc_341.cpp	Mon Oct 06 19:31:52 2014 +0000
@@ -7,12 +7,25 @@
  Looing at vector product of the x-y components of the accelerometer.
  Works pretty well. Still rough, program wise - sc 140710
  */
-
+// solution UG 1
+#define NUMLEDS 2
+#define PORTRAIT 0
+#define PORTRAIT1 1 // Add bandwidth
+#define LANDSCAPE 10
+#define LANDSCAPE1 9
+#define LEDON 0
+#define LEDOFF 1
+#define RED 0
+#define GREEN 1
+#define ACCSCALING 10.0
+#define HYSTOFFSET 4
+DigitalOut LEDs[NUMLEDS]={LED_RED, LED_GREEN}; // plan for scalability
+// end UG 1
 #define DATATIME 0.200
 
 #define PROGNAME "ACCLCD341\r/n"
 
-#define PRINTDBUG
+// #define PRINTDBUG
 // 
 #if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
   PinName const SDA = PTE25;  // Data pins for the accelerometer/magnetometer.
@@ -32,7 +45,24 @@
 Serial pc(USBTX, USBRX);
 
 float sqrt_newt(float argument) {
-    return (sqrt(argument));
+    int i = 0;
+    float xnew = 0.0;
+    int itermax = 20;
+    float epsilon = 1e-7;
+    float xold = argument/2.0;
+    float delta = 1;
+    while ((delta > epsilon) && (i < itermax)){
+        xnew = 0.5*(xold + (argument/xold));
+        delta = abs(xnew-xold);      
+        xold = xnew;
+        i++;
+#ifdef PRINTDBUG
+        // wait(0.1);
+        pc.printf("%5.4f %5.4e\r\n",xold, delta);
+#endif       
+    } // end while
+    return (xold);
+    
 }
    
 
@@ -45,33 +75,76 @@
 
 
 int main() {
+    int i;
     float xAcc;
     float yAcc;
+    float zAcc;
     float vector;  
+    int positionState;
     char lcdData[10]; //buffer needs places dor decimal pt and colon
+    Timer DATATimer; 
+    int hyst = HYSTOFFSET;
+    
+    
     
 #ifdef PRINTDBUG
         pc.printf(PROGNAME);
 #endif
+    DATATimer.start();
+    DATATimer.reset();
 // main loop forever 
     while(true) {
-
+        while (DATATimer.read_ms() >  DATATIME) {
+        
 //Get accelerometer data - tilt angles minus offset for zero mark.
-        xAcc = abs(acc.getAccX());
-        yAcc = abs(acc.getAccY());     
+            xAcc = abs(acc.getAccX());
+            yAcc = abs(acc.getAccY()); 
+            zAcc = abs(acc.getAccZ());       
  // Calulate vector sum of x and y reading.       
-        vector = sqrt_newt(pow(xAcc,2) + pow(yAcc,2));
+            vector = sqrt_newt(pow(xAcc,2) + pow(yAcc,2));
+            // Calculate the state based on orientaion
+            positionState = int(ACCSCALING * xAcc) + hyst; // Create a Deadband
+            if (positionState >= LANDSCAPE){
+                positionState = LANDSCAPE;
+            } else {
+                positionState = PORTRAIT;
+            }
+                
+// State Machine
+            switch (positionState){
+                case LANDSCAPE:
+                case LANDSCAPE1: {
+                    LEDs[RED].write(LEDON);
+                    LEDs[GREEN].write (LEDOFF);
+                    hyst = HYSTOFFSET;
+                    break;
+                }
+                case  PORTRAIT:
+                case  PORTRAIT1: {
+                    LEDs[RED].write(LEDOFF);
+                    LEDs[GREEN].write (LEDON);
+                    hyst= 0;
+                    break;
+                }
+                default: {
+                    for (i = 0; i< NUMLEDS; i++){
+                        LEDs[i].write(LEDON);
+                    }
+                    break;
+                }
+            }  //switch
         
- 
 #ifdef PRINTDBUG
         pc.printf("xAcc = %f\r\n", xAcc);
         pc.printf("yAcc = %f\r\n", yAcc);
+        pc.printf("xAcc = %f\r\n", zAcc);
         pc.printf("vector = %f\r\n",  vector);
 #endif
 
-        sprintf (lcdData,"%4.3f",vector);
-        LCDMess(lcdData, DATATIME);
+            sprintf (lcdData,"%4.3f",vector);
+            LCDMess(lcdData, vector);
+            DATATimer.reset();
 // Wait then do the whole thing again.
-        wait(DATATIME);
-    }
+        } // end whle for timer
+    }// end forever while
 }
\ No newline at end of file