Revision 4:78a6a4330690, committed 2019-02-21

Comitter:

gtanvir

Date:

Thu Feb 21 21:35:03 2019 +0000

Parent:

3:d5c746840139

Commit message:

Final, Viva(ed)

Changed in this revision

diff -r d5c746840139 -r 78a6a4330690 main.cpp
--- a/main.cpp	Wed Jan 30 21:54:03 2019 +0000
+++ b/main.cpp	Thu Feb 21 21:35:03 2019 +0000
@@ -6,14 +6,30 @@
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
+#define intermediateState 0
+#define flatState 1
+#define rightState 2
+#define leftState 3
+#define downState 4
+#define upState 5
+#define overState 6
+
+#define threshold 0.90
+
+DigitalOut redLed(LED1);
+DigitalOut greenLed(LED2);
+DigitalOut blueLed(LED3);
+
 int main(void)
 {
+    uint8_t systemState = flatState;
+    int counter =0, error = 0, correction = 0;
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
-    PwmOut rled(LED1);
-    PwmOut gled(LED2);
-    PwmOut bled(LED3);
+    //PwmOut rled(LED1);
+    //PwmOut gled(LED2);
+    //PwmOut bled(LED3);
     Serial pc(USBTX, USBRX); // tx, rx
-
+    
 
     pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
 
@@ -22,10 +38,91 @@
         x = acc.getAccX();
         y = acc.getAccY();
         z = acc.getAccZ();
-        rled = 1.0f - abs(x);
-        gled = 1.0f - abs(y);
-        bled = 1.0f - abs(z);
-        wait(0.3);
-        pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
+        
+        switch(systemState) {
+            case flatState:
+                if(abs(z)<threshold) {
+                    systemState = intermediateState;
+                }
+                redLed.write(0);
+                greenLed.write(1);
+                blueLed.write(1); 
+                break;
+            case overState:
+                if(abs(z)<threshold) {
+                    systemState = intermediateState;
+                }
+                redLed.write(1);
+                greenLed.write(0);
+                blueLed.write(1);
+                break;
+            case rightState:
+                if(abs(y)<threshold) {
+                    systemState = intermediateState;
+                }
+                redLed.write(1);
+                greenLed.write(1);
+                blueLed.write(0);
+                break;
+            case leftState:
+                if(abs(y)<threshold) {
+                    systemState = intermediateState;
+                }
+                redLed.write(0);
+                greenLed.write(0);
+                blueLed.write(1);
+                break;
+            case downState:
+                if(abs(x)<threshold) {
+                    systemState = intermediateState;
+                }
+                redLed.write(1);
+                greenLed.write(0);
+                blueLed.write(0);
+                break;
+            case upState:
+                if(abs(x)<threshold) {
+                    systemState = intermediateState;
+                }
+                redLed.write(0);
+                greenLed.write(1);
+                blueLed.write(0);
+                break;
+            case intermediateState:
+                if(abs(x)>threshold && x>0) {
+                    systemState = upState;
+                    pc.printf("Up State\r\n");
+                    }
+                else if(abs(x)>threshold && x<0) {
+                    systemState = downState;
+                    pc.printf("Down State\r\n");
+                    }
+                else if(abs(y)>threshold && y>0) {
+                    systemState = leftState;
+                    pc.printf("Left State\r\n");
+                    }
+                else if(abs(y)>threshold && y<0) {
+                    systemState = rightState;
+                    pc.printf("Right State\r\n");
+                    }
+                else if(abs(z)>threshold && z>0) {
+                    systemState = flatState;
+                    pc.printf("Flat State\r\n");
+                    }
+                else if(abs(z)>threshold && z<0) {
+                    systemState = overState;
+                    pc.printf("Over State\r\n");
+                    }
+                redLed.write(1);
+                greenLed.write(1);
+                blueLed.write(1);
+                break;
+                
+        }
+        
+        wait(0.1);
+        counter++;
+        
+        //pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
     }
 }