Diff: OperationMode.cpp

Revision:

15:97311058e012

Parent:

14:771cd444764b

Child:

16:1c8a8643260f

--- a/OperationMode.cpp	Thu Nov 01 09:26:12 2018 +0000
+++ b/OperationMode.cpp	Thu Nov 01 09:38:25 2018 +0000
@@ -26,7 +26,7 @@
 DigitalOut LedBlue(LED_BLUE);
 
 double PositionRef=0;
-double PositionRef2=0;
+double q2Ref =0;
 
 
 //Define substates Operation Mode
@@ -34,102 +34,82 @@
 enum States {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming};
 States CurrentOperationState;
 
+//State Machine for OperationMode
 void OperationStateMachine()
 {
     switch (CurrentOperationState) {
         case OPWait:
-            LedRed = 0; //red
-            LedGreen = 1;
-            LedBlue = 1;
-            if (BUT1 == false) {
+            if (EMGBoolLeft == true) {    //When Left Biceps are contracted, initiate chain to flip the page
                 CurrentOperationState = OPState1;
                 TimerLoop.reset();
             }
-            if (button_sw3 == false) {
+            if (BUTSW3 == false) {  //When sw3 is pressed, go to homing for Operation Mode
                 CurrentOperationState = OPSet;
                 TimerLoop.reset();
             }
             break;
             
-        case OPSet:
-            LedRed = 1; //turqoise
-            LedGreen = 0;
-            LedBlue = 0;
-            if (PositionRef2 > -0.733*(6.380)) {
-                PositionRef2 -= 0.0005*(6.380);
+        case OPSet:                 // Set new homing for Operation Mode and go back to waiting mode
+            if (q2Ref  > -0.733*(6.380)) {
+                q2Ref  -= 0.0005*(6.380);
                 }
-            if (PositionRef2 < -0.733*(6.380) && TimerLoop > 2) {
+            if (q2Ref  < -0.733*(6.380) && TimerLoop > 2) {
                 CurrentOperationState = OPWait;
                 TimerLoop.reset();
                 }
             break;
                 
-        case OPState1:
-            LedRed = 0; //yellow
-            LedGreen = 0;
-            LedBlue = 1;
-            if (PositionRef < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
-                PositionRef += 0.0005*(6.380);
+        case OPState1:                      // First step of chain to flip the page
+            if (q1Ref < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
+                q1Ref += 0.0005*(6.380);
             }
-            if ((PositionRef > 0.48*(6.380)) && (TimerLoop >= 2.0)) {
+            if ((q1Ref > 0.48*(6.380)) && (TimerLoop >= 2.0)) {
                 CurrentOperationState = OPState2;
                 TimerLoop.reset();
             }
             break;
             
-        case OPState2:
-            LedRed = 0; //white
-            LedGreen = 0;
-            LedBlue = 0;
-            if (PositionRef2 > -1.133*(6.380)) {
-                PositionRef2 -= 0.0005*(6.380);
+        case OPState2:                      //Second step of chain to flip the page
+            if (q2Ref  > -1.133*(6.380)) {
+                q2Ref  -= 0.0005*(6.380);
             }
-            if ((PositionRef2 < -1.133*(6.380)) && (TimerLoop >= 2.0)) {
+            if ((q2Ref  < -1.133*(6.380)) && (TimerLoop >= 2.0)) {
                 CurrentOperationState = OPState3;
                 TimerLoop.reset();
             }
             break;
             
-        case OPState3:
-            LedRed = 0; //pink
-            LedGreen = 1;
-            LedBlue = 0;
-            if (PositionRef > -0.15*(6.380)) {
-                PositionRef -= 0.0005*(6.380);
+        case OPState3:                  //Third step of chain to flip the page
+            if (q1Ref > -0.15*(6.380)) {
+                q1Ref -= 0.0005*(6.380);
             }
-            if (PositionRef2 > -1.483*(6.380)) {
-                PositionRef2 -= 0.0003*(6.380);
+            if (q2Ref  > -1.483*(6.380)) {
+                q2Ref  -= 0.0003*(6.380);
             }
-            if ((PositionRef < -0.15*(6.380)) && (PositionRef2 < -1.483*(6.380)) && (TimerLoop >= 3.0)) {
+            if ((q1Ref < -0.15*(6.380)) && (q2Ref  < -1.483*(6.380)) && (TimerLoop >= 3.0)) {
                 CurrentOperationState = OPState4;
                 TimerLoop.reset();
             }
             break;
             
-        case OPState4:
-            LedRed = 1; //blue
-            LedGreen = 1;
-            LedBlue = 0;
-            if (PositionRef2 > -2.133*(6.380)) {
-                PositionRef2 -= 0.005*(6.380);
+        case OPState4:                          //Fourth step of chain to flip the page
+            if (q2Ref  > -2.133*(6.380)) {
+                q2Ref  -= 0.005*(6.380);
             }
-            if ((PositionRef2 < -2.133*(6.380)) && (TimerLoop > 0.5)) {
+            if ((q2Ref  < -2.133*(6.380)) && (TimerLoop > 0.5)) {
                 TimerLoop.reset();
                 CurrentOperationState = OPHoming;
             }
             break;
             
-        case OPHoming:
-            LedGreen = 0; //Green
-            LedBlue = 1;
-            LedRed = 1;
-            if (PositionRef < 0) {
-                PositionRef += 0.003*(6.380);
+        case OPHoming:                          //Go back to Home for Operation Mode and back to Waiting
+            if (q1Ref < 0) {
+                q1Ref += 0.003*(6.380);
             }
-            if (PositionRef2 < -0.733*(6.380)) {
-                PositionRef2 += 0.001*(6.380);
+            if (q2Ref  < -0.733*(6.380)) {
+                q2Ref  += 0.001*(6.380);
             }
-            if ((PositionRef > 0) && (PositionRef2 > -0.733*(6.380)) && (TimerLoop > 3)) {
+            if ((q1Ref > 0) && (q2Ref  > -0.733*(6.380)) && (TimerLoop > 3)) {
                 CurrentOperationState = OPWait;
             }
             break;
@@ -256,14 +236,14 @@
 void MeasureAndControl(void)
 {
 
-    //double PositionRef = GetReferencePosition();
-    //double PositionRef2 = GetReferencePosition2();
+    //double q1Ref = GetReferencePosition();
+    //double q2Ref  = GetReferencePosition2();
 
     OperationStateMachine();
     double PositionMotor = GetActualPosition();
     double PositionMotor2 = GetActualPosition2();
-    double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
-    double MotorValue2 = PID_Controller2(PositionRef2 - PositionMotor2);
+    double MotorValue = PID_Controller(q1Ref - PositionMotor); // input is error
+    double MotorValue2 = PID_Controller2(q2Ref  - PositionMotor2);
     SetMotor(MotorValue, MotorValue2);
 }