Tommie Verouden / Mbed 2 deprecated StateMachine

Dependencies:   biquadFilter FastPWM MODSERIAL QEI mbed

Revision:
45:d405103e9625
Parent:
42:bb43f1b67787
Child:
46:f4dcfe6652ac
--- a/main.cpp	Thu Nov 01 14:58:02 2018 +0000
+++ b/main.cpp	Thu Nov 01 15:26:50 2018 +0000
@@ -426,10 +426,10 @@
     return  u_k + u_i + u_d;
 }
 
-//float angleDesiredL()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-//{   float   angle = (pot2*2.0f*PI)-PI;
-//    return  angle;
-//}
+float angleDesiredL()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+{   float   angle = (pot1*2.0f*PI)-PI;
+    return  angle;
+}
 
 float countsCalibrCalcL(int countsOffsetL)
 {
@@ -496,10 +496,10 @@
     return  u_k + u_i + u_d;
 }
 
-//float angleDesiredR()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-//{   float   angle = (pot2*2.0f*PI)-PI;
-//    return  angle;
-//}
+float angleDesiredR()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+{   float   angle = (pot2*2.0f*PI)-PI;
+    return  angle;
+}
 
 float countsCalibrCalcR(int countsOffsetR)
 {
@@ -637,12 +637,13 @@
 //      ----------------------------- action ------------------------------
                     // Actions for each loop iteration
                     /* */
-
+                    calibrationL();
+                    calibrationR();
 //      --------------------------- transition ----------------------------
                     // Transition condition: when all motor errors smaller than 0.01,
                     //      start calibrating EMG
-                    if (errorMotorL < 0.01 && errorMotorR < 0.01
-                            && errorMotorF < 0.01 && buttonHome == false) {
+                    if (errorL < 0.01 && errorR < 0.01
+                            && errorF < 0.01 && buttonHome == false) {
 
                         // Actions when leaving state
                         blinkTimer.detach();
@@ -682,7 +683,7 @@
 
 //      --------------------------- transition ----------------------------
                     // Transition condition: after 20 sec in state
-                    if (local_timer.read() > 20) {
+                    if (EMGtransistion_timer.read() > 20) {
                         // Actions when leaving state
                         blinkTimer.detach();
 
@@ -711,20 +712,20 @@
                     /* */
 
 //      --------------------------- transition ----------------------------
-                    // Transition condition #1: with button press, enter demo mode,
+                    // Transition condition #1: with button press, enter reading mode,
                     //      but only when velocity == 0
-                    if (errorMotorL < 0.01 && errorMotorR < 0.01
-                            && errorMotorF < 0.01 && buttonBio1 == true) {
+                    if (errorCalibratedL < 0.01 && errorCalibratedR < 0.01
+                            && errorCalibratedF < 0.01 && buttonBio1 == true) {
                         // Actions when leaving state
                         /* */
 
                         currentState = reading;             // change to state
                         changeState = true;                 // next loop, switch states
                     }
-                    // Transition condition #2: with button press, enter operation mode
+                    // Transition condition #2: with button press, enter demo mode
                     //      but only when velocity == 0
-                    if (errorMotorL < 0.01 && errorMotorR < 0.01
-                            && errorMotorF < 0.01 && buttonBio2 == true) {
+                    if (errorCalibratedL < 0.01 && errorCalibratedR < 0.01
+                            && errorCalibratedF < 0.01 && buttonBio2 == true) {
                         // Actions when leaving state
                         /* */
 
@@ -756,13 +757,13 @@
                     /* */
 
 //      --------------------------- transition ----------------------------
-                    // Transition condition #1: when EMG signal detected, enter reading
+                    // Transition condition #1: when EMG signal detected, enter operating
                     //      mode
-                    if (xMove == true || yMove == true) {
+                    if (xMove == true && yMove == true) {
                         // Actions when leaving state
                         /* */
 
-                        currentState = reading;             // change to state
+                        currentState = operating;             // change to state
                         changeState = true;                 // next loop, switch states
                     }
                     // Transition condition #2: with button press, back to homing mode
@@ -796,7 +797,7 @@
 
 //      --------------------------- transition ----------------------------
                     // Transition condition: when path is over, back to reading mode
-                    if (errorMotorL < 0.01 && errorMotorR < 0.01) {
+                    if (errorCalibratedL < 0.01 && errorCalibratedR < 0.01) {
                         // Actions when leaving state
                         blinkTimer.detach();
 
@@ -824,6 +825,9 @@
                     /* */
                     ReadUseEMG0();                  //Start the use of EMG
                     ReadUseEMG1();                  //Start the use of EMG
+                    kinematics();
+                    motorTurnL();
+                    motorTurnR();
 
 //      --------------------------- transition ----------------------------
                     // Transition condition: with button press, back to homing mode