Diff: main.cpp

Revision:

3:1861f741c2df

Parent:

2:304358fa24aa

--- a/main.cpp	Fri Feb 16 21:58:20 2018 +0000
+++ b/main.cpp	Wed Feb 28 14:24:52 2018 +0000
@@ -2,12 +2,12 @@
 #include "rtos.h"
 #include "MMA8451Q.h"
 
-  PinName const SDA = PTE25;
-  PinName const SCL = PTE24;
+PinName const SDA = PTE25;
+PinName const SCL = PTE24;
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
-
+Timer t;
 int main(void)
 {
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
@@ -15,19 +15,98 @@
     PwmOut gled(LED2);
     PwmOut bled(LED3);
     Serial pc(USBTX, USBRX); // tx, rx
-
+    enum s { flat, right, up,error};
+    s state=flat;
 
     pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
 
     while (true) {
         float x, y, z;
+        double th,time1,time2,time3;
+        th=0.5;
         x = acc.getAccX();
         y = acc.getAccY();
         z = acc.getAccZ();
-        rled = 1.0f - abs(x);
-        gled = 1.0f - abs(y);
-        bled = 1.0f - abs(z);
+        //rled = 1.0f - abs(x);
+        //gled = 1.0f - abs(y);
+        // bled = 1.0f - abs(z);
+        if (z>0.5) {
+            state=flat;
+        } else if ((x>th)&&(abs(y)<th)&&(abs(z)<th)) {
+            state=right;
+        } else if ((y>th)&&(abs(x)<th)&&(abs(z)<th)) {
+            state=up;
+        } else {
+            state=error;
+        }
+        switch (state) {
+            case flat:
+                t.start();
+                gled=0;
+                bled=1;
+                rled=1;
+                pc.printf("flat");
+                t.stop();
+                time1=t.read();
+                t.reset();
+                if (time1>=10) {
+                    state=right;
+                } else {
+                    state=error;
+                }
+                break;
+            case right:
+                gled=1;
+                bled=1;
+                rled=1;
+                t.start();
+                pc.printf("right");
+                t.stop();
+                time2=t.read();
+                t.reset();
+                if ((time2>2)&&(time2<6)) {
+                    state=up;
+                } else {
+                    state=error;
+                }
+                break;
+            case up:
+                gled=1;
+                bled=1;
+                rled=1;
+                t.start();
+                pc.printf("up");
+                t.stop();
+                time3=t.read();
+                t.reset();
+                if ((time3>4)&&(time3)<8) {
+                    state=flat;
+                } else {
+                    state=error;
+                }
+                break;
+            case error:
+                printf("error");
+                rled=0;
+                gled=1;
+                bled=1;
+                break;
+
+
+
+
+
+
+        }
+
+
+
+
+
+
+
+
         Thread::wait(300);
-        pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
+        //pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n \r", x, y, z);
     }
 }