Revision 6:65d8a80eb0b6, committed 2020-03-05

Comitter:

ex19397

Date:

Thu Mar 05 23:04:02 2020 +0000

Parent:

5:392ac6e02d80

Commit message:

Final

Changed in this revision

--- a/main.cpp	Thu Feb 27 10:07:17 2020 +0000
+++ b/main.cpp	Thu Mar 05 23:04:02 2020 +0000
@@ -5,7 +5,32 @@
   PinName const SCL = PTE24;
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
-
+int getOrientation(float x, float y, float z) 
+{
+    int retval = 6;
+    if (z > 0.9) // State = Flat
+    {
+        retval = 0;
+    } else if (x < -0.9) //state is up
+    {
+        retval = 1;
+    } else if (y < -0.9)  //State = Right
+    { 
+    
+        retval = 2;
+    } else if (y > 0.9) // state is left
+    {
+        retval = 3;
+    } else if (x > 0.9) //state is Down
+    {
+        retval = 4;
+    } else if (z < -0.9) //state is over
+    {
+        retval = 5;
+    } 
+     
+    return retval;
+}
 int main(void)
 {
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
@@ -13,19 +38,309 @@
     PwmOut gled(LED2);
     PwmOut bled(LED3);
     Serial pc(USBTX, USBRX); // tx, rx
+    enum states {FLAT, UP, RIGHT, LEFT, DOWN, OVER, INTERMEDIATE};
+    
+    int devFlat = 0;
+    int devUp = 0;
+    int devRight = 0;
+    bool sequence = true;
+    bool trigger = false;
+    float elapsed = 0;
+    Timer t;
+    t.start(); //incase t.read() throws an error in the first loop 
+    
 
 
     pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
 
     while (true) {
         float x, y, z;
+        int devState = 0;
         x = acc.getAccX();
         y = acc.getAccY();
         z = acc.getAccZ();
-        rled = 1.0f - abs(x);
+          
+        //enum states deviceState;
+        devState = getOrientation(x,y,z);
+        
+        if (sequence == false ) 
+        {
+            pc.printf("Invalid Sequence. Sequence is broken \n\r");
+            if (devState == FLAT) {
+                t.stop();
+                elapsed = t.read();
+                if (elapsed <= 4) {
+                    pc.printf("Sequence is broken. Keep flat for 10 Seconds to reset %f\n\r", elapsed);
+                    t.start();
+                } else {
+                    pc.printf("Resetting is complete %f\n\r", elapsed);
+                    sequence = true;
+                    t.reset();
+                    t.start();
+                    devUp = 0;
+                    devRight = 0;
+                    devFlat = 0;
+                }
+            } else {
+                t.stop();
+                elapsed = t.read();
+                pc.printf("Device must be flat for 10 seconds, %f \n", elapsed);
+                t.reset();
+                t.start();
+                sequence = false;
+            }
+        }
+        else     
+        {     
+        if ((devFlat == 0) && (devUp == 0) && (devRight == 0)) //Check if device is initialized with no state
+        {
+            if (devState == FLAT) { //Check if device is flat
+                t.stop();
+                elapsed = t.read();
+                if (elapsed < 10 ) { // ... check if it is still under 10 secs
+                    sequence = true;  
+                    t.start();     
+                    pc.printf("Correct State. State is FLAT Time is < 10 SECS %d %f\n\r", devState, elapsed);
+                    rled = 1;
+                    gled = 0;
+                    bled = 1;            
+                } else {
+                    pc.printf("10 seconds have elapsed, expect transition %d %f\n\r", devState, elapsed);   
+                    devFlat = 1; // 10 secs has elapsed, move to the next state
+                    t.start();
+                }         
+            } else if (devState == RIGHT) { // state has changed to right
+                t.stop();
+                elapsed = t.read();
+                if (elapsed > 10 ) // It is over 10 seconds 
+                {
+                    pc.printf("10 seconds have elapsed, State is now right %d %f\n\r", devState, elapsed);  
+                    sequence = true;
+                    devFlat = 1;
+                    t.reset();
+                    t.start();
+                } else  { // State changed to right before 10 secs elapsed
+                    sequence = false;
+                    pc.printf("Incorrect state. Turned right too early, State is now right %d %f\n\r", devState, elapsed);  
+                    t.stop();
+                    t.reset();
+                    t.start();
+                }
+                
+            } 
+            else if (devState == INTERMEDIATE) {
+                pc.printf("Intermediate State. %f\n\r", elapsed);
+                sequence = true;
+            } 
+            else { //This is the invalid state in this case. 
+                sequence = false;
+                t.stop();
+                elapsed = t.read();
+                pc.printf("Invalid State, State should be FLAT or RIGHT after 10 secs %f\n\r", elapsed); 
+                t.reset();
+                t.start();
+            }
+        } else if ((devFlat  == 1) && (devRight == 0) && (devUp == 0)) //check that the previous state was flat
+        {
+            if (devState == RIGHT)  // Device is in the correct state. 
+            {
+                t.stop();
+                elapsed = t.read();
+                if (elapsed > 10) { // ... but check if it entered this state after the elapsed time
+                    sequence = true;
+                    t.reset(); // reset the previous timer
+                    t.start();
+                    pc.printf("Correct State. State is RIGHT TIME IS > 10 SECS %d %f\n\r", devState, elapsed);
+                } else if ((elapsed > 2) && (elapsed <= 6)) {                 
+                    t.start();
+                    pc.printf("Correct State. State is RIGHT: %d, Time is %f \n\r", devState, elapsed);
+                    sequence = true;
+                }  else if (elapsed < 2) { // still in right, do nothing
+                    t.start();
+                    rled = 1;
+                    gled = 1;
+                    bled = 1; 
+                    pc.printf("Correct State. State is RIGHT, Time is  %f \n\r", elapsed);
+                    sequence = true;
+                } else if (elapsed > 6) {
+                    devRight = 1;
+                    t.reset();
+                    t.start();
+                    sequence = true;
+                    pc.printf("Correct State. Expecting new state. time is  %f \n\r", elapsed);
+                }          
+                
+            }  else if (devState == FLAT) //it will never enter this state 
+            {
+                t.stop();
+                elapsed = t.read();
+               
+                if (elapsed > 10 ) { //check if it is within the expected duration
+                    sequence = true;
+                     pc.printf("Correct State. State is still Flat: %d, However TIME > 10 SECS Timer is %f \n\r", devState, elapsed);
+                     rled = 1;
+                     gled = 0;
+                     bled = 1;   
+                    t.start();
+                } else {
+                    pc.printf("Incorrect State. I shouldn't even be here. State is RIGHT: %d, BUT TIME < 10 SECS Timer is %f \n\r", devState, elapsed);
+                    rled = 0;
+                    gled = 1;
+                    bled = 1;   
+                    t.reset();
+                    t.start();
+                    sequence = false;
+                }
+            }  
+            else if (devState == INTERMEDIATE) {
+                pc.printf("Intermediate State. %f\n\r", elapsed);
+                sequence = true;
+            }
+            else { //Device is in the wrong state
+             pc.printf("Inorrect State. State should be flat or right: %d, and TIME < 10 SECS Timer is %f \n\r", devState, elapsed);
+                t.stop();
+                t.reset();
+                t.start();
+                sequence = false;
+                rled = 0;
+                gled = 1;
+                bled = 1; 
+            }
+        } else if ((devFlat == 1) && (devRight == 1) && (devUp == 0)) //DO THE SAME THING FOR THIS ONE
+        {
+            if (devState == UP)  // Device is in the correct state
+            {
+                t.stop();
+                elapsed = t.read();
+                if (elapsed < 9) { // ... but check if it entered this state after the elapsed time (fix this one o)
+                    t.start();
+                    rled = 1;
+                    gled = 1;
+                    bled = 1; 
+                    pc.printf("Correct State. State is UP %d and time is %f \n\r", devState, elapsed);
+                    sequence = true;
+                } else {                 
+                    t.reset();
+                    t.start();
+                    devUp = 1;
+                    sequence = true;
+                    pc.printf("Sequence Complete! %f\n\r", elapsed);
+                }
+                
+            }  
+            else if (devState == RIGHT) {
+                t.stop();
+                elapsed = t.read();
+                if (elapsed <= 3) {
+                    pc.printf("Go up Now. %f\n\r", elapsed);
+                    t.start();
+                    sequence = true;
+                } else {
+                    sequence = false;
+                    t.reset();
+                    t.start();
+                    pc.printf("Cannot help you. Should have gone up. %f\n\r", elapsed);
+                }
+            }
+            else if (devState == INTERMEDIATE) {
+                pc.printf("Intermediate State. %f\n\r", elapsed);
+                sequence = true;
+            }
+            else 
+            {   
+                rled = 0;
+                gled = 1;
+                bled = 1; 
+                
+                t.stop();
+                elapsed = t.read();
+                if ((elapsed < 4) ) {
+                    pc.printf("Incorrect State. State is NOT UP %d and time is %f \n\r", devState, elapsed); // its entering this state
+                    t.reset();
+                    sequence = false;
+                    t.start();
+                    
+                } else {
+                     pc.printf("WHERE AM I %d and time is %f \n\r", devState, elapsed);
+                     t.reset();
+                     t.start();
+                }
+                
+            }
+        } else if ((devFlat == 1) && (devRight == 1) && (devUp == 1)) //Last State
+        {
+            if (devState == FLAT)  // Device is flat (correct state)
+            {
+                t.stop();
+                elapsed = t.read();            
+                if (elapsed  < 10) { // ... if time is less than 10 secs, do nothing
+                    t.start();
+                    pc.printf("Correct State Device should be flat for 10 secs, %f\n\r", elapsed);
+                    sequence = true;
+                } else {       // if time is >= 10 seconds do trigger reset all states
+                    pc.printf("Trigger Activated. State is %d \n\r", devState);
+                    trigger = true;
+                    t.reset();           
+                    t.start();
+                    sequence = true;
+                    devFlat = 0;
+                    devUp = 0;
+                    devRight = 0;
+                }
+                
+            } 
+             else if (devState == INTERMEDIATE) {
+                pc.printf("Intermediate State. %f\n\r", elapsed);
+                sequence = true;
+            }
+             else  //device is not flat, show error
+            {
+                t.stop();
+                elapsed = t.read();
+                t.reset();
+                pc.printf("Device should be flat for 10 seconds. State is %d, timer is %f \n\r", devState, elapsed);
+                // measure time. If time has elapsed, sequence is false else sequence is true
+            }
+            }
+            else 
+            {
+                t.stop();
+                t.start();
+                sequence = false;
+            }
+        }
+    
+        if (sequence == true) 
+        {
+           // pc.printf("Sequence Correct %d, Timer: %f\n\r", devState, elapsed);
+        } else {
+           // pc.printf("Sequence False %d, Timer:  %f\n\r", devState, elapsed);
+        }
+        
+        if (trigger == true) { //do trigger thing
+        }
+        
+        
+        ThisThread::sleep_for(300);
+        
+       /* if (orientation == flat) 
+        {
+            pc.printf("flat\n");
+        } else if (orientation == up)
+        {
+            pc.printf("up\n");
+        } else if (orientation == right) 
+        {
+            pc.printf("right\n");
+        } else {
+            pc.printf("Unknown State\n");
+        }*/
+        
+        /*rled = 1.0f - abs(x);
         gled = 1.0f - abs(y);
-        bled = 1.0f - abs(z);
-        ThisThread::sleep_for(300);  // wait(0.3);
-        pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
+        bled = 1.0f - abs(z);*/
+       
+        //ThisThread::sleep_for(300);  // wait(0.3);
+        //pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
     }
 }