Diff: main.cpp

Revision:

2:8d75d6728722

Parent:

0:a1caba5c4e48

--- a/main.cpp	Wed Feb 07 16:56:55 2018 +0000
+++ b/main.cpp	Thu Feb 22 17:41:08 2018 +0000
@@ -1,32 +1,265 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "MMA8451Q.h"
+ 
+PinName const SDA = PTE25;
+PinName const SCL = PTE24;
+ 
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+ 
+Thread isTriggeredStatus ;
+ 
+enum states { FLAT, RIGHT, LEFT, DOWN, UP, OVER, INTERMEDIATE };
+ 
+Serial pc(USBTX, USBRX); // tx, rx
+ 
+volatile states positionState = INTERMEDIATE;
+ 
+void ifTrigerred()
+{
+    DigitalOut rled(LED1);
+    DigitalOut gled(LED2);
+ 
 
-  PinName const SDA = PTE25;
-  PinName const SCL = PTE24;
-
-#define MMA8451_I2C_ADDRESS (0x1d<<1)
-
+    enum spaceStates { flatstart, flatEnd, rightstart, rightEnd, upstart, upEnd, flatTriggered, stateTimeError, stateSequenceError };
+    Timer timer;
+ 
+    rled = 1;
+    gled = 1;
+    spaceStates state = flatstart;
+ 
+    while (true) {
+        if (positionState == INTERMEDIATE) {
+            Thread::wait(100);
+            continue;
+        }
+ 
+ 
+        switch (state) {
+            case flatstart:
+                if (positionState == FLAT) {
+                    pc.printf("flatstart \n\r");
+                    timer.start();
+                    pc.printf("%d", timer.read_ms());
+                    if (timer.read() >= 10) {
+                        timer.stop();
+                        timer.reset();
+                        state = flatEnd;
+                    }
+                } else {
+                    timer.stop();
+                    timer.reset();
+                    if (positionState == RIGHT) {
+                        state = stateTimeError;
+                    } else {
+                        state = stateSequenceError;
+                    }
+                }
+                break;
+ 
+            case flatEnd:
+                rled = 1;
+                pc.printf("FlatEnd \n\r");
+                if (positionState == RIGHT) {
+                    state = rightstart;
+                } else {
+                    if (positionState != FLAT) {
+                        state = stateSequenceError;
+                    }
+                }
+                break;
+ 
+            case rightstart:
+                if (positionState == RIGHT) {
+                    pc.printf("RightStart \n\r");
+                    timer.start();
+                    pc.printf("%d", timer.read_ms());
+                    if (timer.read() >= 2) {
+                        state = rightEnd;
+                    }
+                } else {
+                    timer.stop();
+                    timer.reset();
+                    if (positionState == UP) {
+                        state = stateTimeError;
+                    } else {
+                        state = stateSequenceError;
+                    }
+                }
+                break;
+ 
+            case rightEnd:
+                pc.printf("RightEnd \n\r");
+                pc.printf("%d", timer.read_ms());
+                if (timer.read() <= 6) {
+                    if (positionState == UP) {
+                        timer.stop();
+                        timer.reset();
+                        state = upstart;
+                    } else if (positionState != RIGHT) {
+                        timer.stop();
+                        timer.reset();
+                        state = stateSequenceError;
+                    }
+                } else {
+                    timer.stop();
+                    timer.reset();
+                    state = stateTimeError;
+                }
+                break;
+ 
+            case upstart:
+                if (positionState == UP) {
+                    pc.printf("UpStart \n\r");
+                    timer.start();
+                    pc.printf("%d", timer.read_ms());
+                    if (timer.read() >= 4) {
+                        state = upEnd;
+                    }
+                } else {
+                    timer.stop();
+                    timer.reset();
+                    if (positionState == FLAT) {
+                        state = stateTimeError;
+                    } else {
+                        state = stateSequenceError;
+                    }
+                }
+                break;
+ 
+            case upEnd:
+                pc.printf("upEnd \n\r");
+                pc.printf("%d", timer.read_ms());
+                if (timer.read() <= 8) {
+                    if (positionState == FLAT) {
+                        timer.stop();
+                        timer.reset();
+                        state = flatTriggered;
+                    } else if (positionState != UP) {
+                        timer.stop();
+                        timer.reset();
+                        state = stateSequenceError;
+                    }
+                } else {
+                    timer.stop();
+                    timer.reset();
+                    state = stateTimeError;
+                }
+                break;
+ 
+            case flatTriggered:
+                gled = 0;
+                pc.printf("flatTriggered \n\r");
+                break;
+ 
+            case stateSequenceError:
+                rled = 0;
+                pc.printf("State sequence error \n\r");
+                if (positionState == FLAT) {
+                    state = flatstart;
+                }
+                break;
+ 
+            case stateTimeError:
+                rled = 0;
+                pc.printf("State time error \n\r");
+                if (positionState == FLAT) {
+                    state = flatstart;
+                }
+                break;
+        }
+ 
+        Thread::wait(100);
+    }
+}
+ 
 int main(void)
 {
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
-    PwmOut rled(LED1);
-    PwmOut gled(LED2);
-    PwmOut bled(LED3);
-    Serial pc(USBTX, USBRX); // tx, rx
-
-
-    pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
-
+ 
+ 
+    states state = INTERMEDIATE;
+    states lastStableState = INTERMEDIATE;
+    states pastState = INTERMEDIATE;
+    int stateCounter = 0;
+    int const stableState = 3;
+ 
+    pc.printf("MMA8451 ID: %d\n\r", acc.getWhoAmI());
+ 
+    // is triggered or not
+    isTriggeredStatus.start(callback(ifTrigerred));
+ 
     while (true) {
-        float x, y, z;
+        float x, y, z, ts;
+ 
+        //threshold
+        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);
-        Thread::wait(300);
-        pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
+ 
+        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 (state != INTERMEDIATE) {
+            if (state == pastState) {
+                if (stateCounter < stableState) {
+                    stateCounter ++;
+                }
+            } else {
+                pastState = state;
+                stateCounter = 0;
+            }
+        }
+ 
+        // state has changed and is stable
+        if (stateCounter == stableState && lastStableState != state) {
+            lastStableState = state;
+ 
+            switch (state) {
+                case FLAT:
+                    pc.printf("Flat state \n\r");
+                    break;
+ 
+                case LEFT:
+                    pc.printf("Left state \n\r");
+                    break;
+ 
+                case RIGHT:
+                    pc.printf("Right state \n\r");
+                    break;
+ 
+                case UP:
+                    pc.printf("Up state \n\r");
+                    break;
+ 
+                case DOWN:
+                    pc.printf("Down state \n\r");
+                    break;
+ 
+                case OVER:
+                    pc.printf("Over state \n\r");
+                    break;
+            }
+ 
+            positionState = state;
+        }
+ 
+        Thread::wait(200);
     }
 }
+            
\ No newline at end of file