Diff: main.cpp

Revision:

3:c8013ce381bc

Parent:

2:b884d78f375d

--- a/main.cpp	Fri Feb 16 22:29:38 2018 +0000
+++ b/main.cpp	Thu Feb 22 15:02:47 2018 +0000
@@ -1,96 +1,285 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "MMA8451Q.h"
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
 
 PinName const SDA = PTE25;
 PinName const SCL = PTE24;
+Thread threadLed, threadAcc;
+Timer t1, t2;
+float timer1, timer2, timer3, totalTimer;
+int seqError;
+int timePrint, statePrint, printReset, printFlat;
+volatile int states;
+volatile int accState;
 
-#define MMA8451_I2C_ADDRESS (0x1d<<1)
 
-int main(void)
+PwmOut rled(LED1);
+PwmOut gled(LED2);
+PwmOut bled(LED3);
+Serial pc(USBTX, USBRX); // tx, rx
+MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+
+
+void stateAcc()
 {
-    MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
-    PwmOut rled(LED1);
-    PwmOut gled(LED2);
-    PwmOut bled(LED3);
-    Serial pc(USBTX, USBRX); // tx, rx
-    accState2=0;
+    t1.start();
+    t2.start();
+    timePrint=1;
+    statePrint=1;
+    float resetTime=10;
+    int prAccState=0;
+    float x, y, z;
+    float accmin=0.95;
+    float accmax=1.05;
+    float acc2min=0.90;
+    float acc2max=1.10;
+    int accloop=1;
+    while (true) {
 
-    pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
-
-    while (true) {
-        float x, y, z;
-        int accState;
-        float accmin=0.95;
-        float accmax=1.05;
         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);
+        if (accloop==1) {
+            if(z>=accmin && z<=accmax) {
+                accState=1;
+            } else if(y<=-(accmin) && y>=-(accmax)) {
+                accState=2;
+            } else if(y>=accmin && y<=accmax) {
+                accState=3;
+            } else if(x<=-(accmin) && x>=-(accmax)) {
+                accState=4;
+            } else if(x>=accmin && x<=accmax) {
+                accState=5;
+            } else if(z<=-(accmin) && z>=-(accmax)) {
+                accState=6;
+            } else {
+                accState=0;
+            }
+        }
+
+        if(accState==1) {
+            if(z>=acc2min && z<=acc2max)accloop=0;
+            else accloop=1;
+        } else if(accState==2) {
+            if(y<=-(acc2min) && y>=-(acc2max))accloop=0;
+            else accloop=1;
+        } else if(accState==3) {
+            if(y>=acc2min && y<=acc2max)accloop=0;
+            else accloop=1;
+        } else if(accState==4) {
+            if(x<=-(acc2min) && x>=-(acc2max))accloop=0;
+            else accloop=1;
+        } else if(accState==5) {
+            if(x>=acc2min && x<=acc2max)accloop=0;
+            else accloop=1;
+        } else if(accState==6) {
+            if(z<=-(acc2min) && z>=-(acc2max))accloop=0;
+            else accloop=1;
+        }
+
+        if(accState!=prAccState) {
+            switch (accState) {
+                case 0: //Intermidiate
+                    pc.printf("positioning\n\r");
+                    break;
+                case 1: //Flat
+                    pc.printf("flat\n\r");
+                    break;
+                case 2: //left
+                    pc.printf("left\n\r");
+                    break;
+                case 3: //right
+                    pc.printf("right\n\r");
+                    break;
+                case 4: //down
+                    pc.printf("down\n\r");
+                    break;
+                case 5: //up
+                    pc.printf("up\n\r");
+                    break;
+                case 6: //over
+                    pc.printf("over\n\r");
+                    break;
+            }
+        }
+        prAccState=accState;
+        
+        if(timePrint==1 && accState!=0)pc.printf("%1.2f s\n\r",t1.read());
+
+        if (states==0) { //normal state
+            rled=1;
+            gled=1;
+            bled=!bled;
+            //do nothing
+        } else if(states==1) { //trigger for green Led
+            rled=1;
+            gled=0;
+            bled=1;
+        } else if(states==2) { //state sequence error
+            if(statePrint==1) {
+                pc.printf("State sequence error!!! please return to flat postion\n\r");
+                statePrint=0;
+            }
+            rled=0;
+            gled=1;
+            bled=1;
+        } else if(states==3) { //state time error
+            if(statePrint==1) {
+                pc.printf("State time error!!! please return to flat postion\n\r");
+                statePrint=0;
+            }
+            rled=0;
+            gled=1;
+            bled=1;
+        } else if(states==4) { //reset state
+            pc.printf("sequence reseting-> wait %1.2f s\n\r",(resetTime-t1.read()));
+            rled=!rled;
+            gled=1;
+            bled=1;
+        }
+        Thread::wait(300);//check where this should be
+    }
+}
 
 
-        if(z>=accmin && z<=accmax) {
-            accState=1;
-        } else if(y<=-(accmin) && y>=-(accmax)) {
-            accState=2;
-        } else if(y>=accmin && y<=accmax) {
-            accState=3;
-        } else if(x<=-(accmin) && x>=-(accmax)) {
-            accState=4;
-        } else if(x>=accmin && x<=accmax) {
-            accState=5;
-        } else if(z<=-(accmin) && z>=-(accmax)) {
-            accState=6;
-        } else {
-            accState=0;
+int main(void)
+{
+    int seq=1;
+    int endprog=0;
+
+    //pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
+
+    rled=1;
+    gled=1;
+    bled=1;
+    //threadLed.start(callback(stateLed));
+    threadAcc.start(callback(stateAcc));
+    Thread::wait(100);
+    /*orient->orentation states
+    orient=1
+    */
+    //flat position
+    while (true) {
+        if (seq==0) {
+            timePrint=0;
+            //statePrint=0;
+            if(accState==1) {
+                states=4;
+                statePrint=1;
+                t1.reset();
+                while(accState==1) {
+                    if(t1.read()>=10) {
+                        t1.reset();
+                        timePrint=1;
+                        //statePrint=1;
+                        seq=1;
+                        t2.reset();
+                        break;
+                    }
+                    //printReset=1;
+                }
+            } else {
+                states=3;
+            }
+        }
+        if(seq==1)pc.printf("Starting Sequence\n\r");
+        while(seq==1) {
+            if (accState==1) {
+                states=0;
+                if(t1.read()<10) {
+                    if(accState!=1) {
+                        states=3;
+                        seq=0;
+                    }
+                } else if (t1.read()>=10) {
+                    states=0;
+                    if(accState!=1)seq=2;
+                }
+            } else if (accState!=1) {
+                states=2;
+                seq=0;
+            }
+        }
+        timer1=t1.read();
+        //positioning
+        while(seq==2) {
+            if(accState>0) {
+                seq=3;
+            }
         }
-        if accState2==accState
-        switch (accState) {
-            case 0: //Intermidiate
-                //
-                rled=1;
-                gled=1;
-                bled=1;
-                break;
-            case 1: //Flat
-                pc.printf("flat\n\r");
-                rled=0;
-                gled=1;
-                bled=1;
+        //right position
+        t1.reset();
+        while(seq==3) {
+            if(accState==3) {
+                states=0;
+                if(t1.read()<2) {
+                    if(accState!=3) {
+                        states=3;
+                        seq=0;
+                    }
+                } else if (t1.read()>=2 && t1.read()<=6) {
+                    states=0;
+                    if(accState!=3)seq=4;
+                } else if (t1.read()>6) {
+                    states=3;
+                    seq=0;
+                }
+            } else if(accState!=3) {
+                states=2;
+                seq=0;
+            }
+        }
+        timer2=t1.read();
+        //positioning
+        while(seq==4) {
+            if(accState>0) {
+                seq=5;
+            }
+        }
+        //up position
+        t1.reset();
+        while(seq==5) {
+            if(accState==5) {
+                states=0;
+                if(t1.read()<4) {
+                    if(accState!=5) {
+                        states=3;
+                        seq=0;
+                    }
+                } else if (t1.read()>=4 && t1.read()<=8) {
+                    states=0;
+                    if(accState!=5)seq=6;
+                } else if (t1.read()>8) {
+                    states=3;
+                    seq=0;
+                }
+            } else if(accState!=3) {
+                states=2;
+                seq=0;
+            }
+        }
+        timer3=t1.read();
+        //positioning
+        while(seq==6) {
+            if(accState>0) {
+                seq=7;
+            }
+        }
+        timePrint=0;
+
+        totalTimer=t2.read();
+        while(seq==7) {
+            if(accState==1) {
+                states=1;
+                t1.stop();
+                t2.stop();
+                endprog=1;
                 break;
-            case 2: //right
-                pc.printf("right\n\r");
-                rled=1;
-                gled=0;
-                bled=1;
-                break;
-            case 3: //left
-                pc.printf("left\n\r");
-                rled=1;
-                gled=1;
-                bled=0;
-                break;
-            case 4: //up
-                pc.printf("up\n\r");
-                rled=0;
-                gled=0;
-                bled=1;
-                break;
-            case 5: //down
-                pc.printf("down\n\r");
-                rled=0;
-                gled=1;
-                bled=0;
-                break;
-            case 6: //over
-                pc.printf("over\n\r");
-                rled=1;
-                gled=0;
-                bled=0;
-                break;
+            }
         }
+        if (seq==7 && endprog==1)break;
     }
-}
+    pc.printf("triggered --- Sequence complete\n\r");
+    pc.printf("flat position:%1.2f s\n\rright position:%1.2f s\n\rup position:%1.2f s\n\rtotal time:%1.2f s\n\r",timer1,timer2,timer3,totalTimer);
+}
\ No newline at end of file