Diff: main.cpp

Revision:

46:f4dcfe6652ac

Parent:

44:ca74d11a2dac

Parent:

45:d405103e9625

Child:

47:7919857587f8

--- a/main.cpp	Thu Nov 01 15:07:51 2018 +0000
+++ b/main.cpp	Thu Nov 01 15:30:27 2018 +0000
@@ -438,10 +438,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)
 {
@@ -508,10 +508,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)
 {
@@ -649,12 +649,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();
@@ -694,7 +695,7 @@
 
 //      --------------------------- transition ----------------------------
                     // Transition condition: after 20 sec in state
-                    if (local_timer.read() > 20) {
+                    if (EMGtransition_timer.read() > 20) {
                         // Actions when leaving state
                         blinkTimer.detach();
 
@@ -723,20 +724,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
                         /* */
 
@@ -768,13 +769,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
@@ -808,7 +809,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();
 
@@ -836,6 +837,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