Diff: main.cpp

Revision:

3:5aa36278a6fe

Parent:

2:dd33a9d29c63

--- a/main.cpp	Fri Feb 16 20:11:08 2018 +0000
+++ b/main.cpp	Thu Feb 22 11:51:39 2018 +0000
@@ -14,43 +14,114 @@
     PwmOut gled(LED2);
     PwmOut bled(LED3);
     Serial pc(USBTX, USBRX); // tx, rx
-
+    
+    enum states { FLAT, RIGHT, LEFT, DOWN, UP, OVER, INTERMEDIATE };
+    states state = INTERMEDIATE;
+    states pastState = INTERMEDIATE;
+    states lastStableState = INTERMEDIATE;
+    int stateCounter = 0;
+    int const stableState = 3;
 
     pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
 
     while (true) {
         float x, y, z, ts;
+ 
         //threshold
-        ts = 0.2;
+        ts = 0.15;
+
         x = acc.getAccX();
         y = acc.getAccY();
         z = acc.getAccZ();
-        rled = 1.0f - abs(x);
-        gled = 1.0f - abs(y);
-        bled = 1.0f - abs(z);
-        
-        // black
-        rled = 1;
-        gled = 1;
-        bled = 1;
         
-        if( abs(x) <= 1+ts && abs(x)>= 1-ts) {
-            rled = 0   ;
+        if (z > 1-ts && z < 1+ts) {
+            state = FLAT;
+        } else if (z > -1-ts && z < -1+ts) {
+            state = OVER;
+        } else if (y > -1-ts && y < -1+ts) {
+            state = RIGHT;
+        } else if (y > 1-ts && y < 1+ts) {
+            state = LEFT;
+        }  else if (x > -1-ts && x < -1+ts) {
+            state = UP;
+        } else if (x > 1-ts && x < 1+ts) {
+            state = DOWN;
+        }  else {
+            state = INTERMEDIATE;
         }
-        if( abs(y) <= 1+ts && abs(y)>= 1-ts) {
-            gled = 0   ;
-        }
-        if( abs(z) <= 1+ts && abs(z)>= 1-ts) {
-            bled = 0   ;
+        
+        if (state != INTERMEDIATE) {
+            if (state == pastState) {
+                if (stateCounter < stableState) {
+                    stateCounter ++;
+                }
+            } else {
+                pastState = state;
+                stateCounter = 0;
+            }
+        } else {
+           // special colour for intermediate level
+           rled = 0.9;
+           gled = 0.9;
+           bled = 0.9;
         }
         
-        if(rled == 0 && gled == 0 && bled ==1)
-            pc.printf("Flat");
-        else if (rled == 0 && gled == 0 && bled ==1)
-            pc.printf("Flat");
+        // print the state
+        if (stateCounter == stableState && lastStableState != state) {
+            lastStableState = state;
+            switch (state) {
+                case FLAT:
+                    pc.printf("Flat state \n\r");
+                    
+                    rled = 0.0;
+                    gled = 0.0;
+                    bled = 1.0;
+                    break;
+                
+                case LEFT:
+                    pc.printf("Left state \n\r");
+                    
+                    rled = 1.0;
+                    gled = 1.0;
+                    bled = 0.0;
+                    break;
+                
+                case RIGHT:
+                    pc.printf("Right state \n\r");
+                    
+                    rled = 1.0;
+                    gled = 0.0;
+                    bled = 0.0;                    
+                    break;
+                
+                case UP:
+                    pc.printf("Up state \n\r");
+                    
+                    rled = 0.0;
+                    gled = 1.0;
+                    bled = 1.0;
+                    break;
+                    
+                case DOWN:
+                    pc.printf("Down state \n\r");
+                    
+                    rled = 0.0;
+                    gled = 1.0;
+                    bled = 0.0;
+                    break;
+                
+               
+               case OVER:
+                    pc.printf("over \n\r");
+                    
+                    rled = 1.0;
+                    gled = 0.0;
+                    bled = 1.0;
+                    break;
+            }
+        }
         
         
         Thread::wait(300);
-       pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n\r", x, y, z);
     }
 }