Diff: OperationMode.cpp

Revision:

11:dcc416dbe3ea

Parent:

10:93957c339972

Child:

12:cae29e41ac2e

--- a/OperationMode.cpp	Wed Oct 31 19:39:57 2018 +0000
+++ b/OperationMode.cpp	Wed Oct 31 21:12:38 2018 +0000
@@ -31,68 +31,105 @@
 
 //Define substates Operation Mode
 
-enum States {OPWait, OPState1, OPState2, OPState3, OPState4, OPHoming};
+enum States {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming};
 States CurrentOperationState;
 
 void OperationStateMachine()
 {
 
-    
+
     switch (CurrentOperationState) {
         case OPWait:
             LedRed = 0; //red
+            LedGreen = 1;
+            LedBlue = 1;
             if (button_sw2 == false) {
+                CurrentOperationState = OPSet;
+                TimerLoop.reset();
+            }
+            break;
+            
+        case OPSet:
+            LedRed = 1; //turqoise
+            LedGreen = 0;
+            LedBlue = 0;
+            if (PositionRef2 > -0.853*(6.380)) {
+                PositionRef2 -= 0.0005*(6.380);
+                }
+            if (PositionRef2 < -0.853*(6.380) && TimerLoop > 2) {
                 CurrentOperationState = OPState1;
                 TimerLoop.reset();
                 }
             break;
+                
         case OPState1:
-            LedGreen = 0; //yellow
-            if (PositionRef < 0.24*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
+            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);
             }
-            if ((PositionRef > 0.24*(6.380)) && (TimerLoop >= 4.0)) {
+            if ((PositionRef > 0.48*(6.380)) && (TimerLoop >= 4.0)) {
                 CurrentOperationState = OPState2;
                 TimerLoop.reset();
             }
             break;
+            
         case OPState2:
-            LedBlue = 0; //white
-            if (PositionRef2 > -0.23*(6.380)) {
+            LedRed = 0; //white
+            LedGreen = 0;
+            LedBlue = 0;
+            if (PositionRef2 > -0.40*(6.380)) {
                 PositionRef2 -= 0.0005*(6.380);
             }
-            if ((PositionRef2 < -0.23*(6.380)) && (TimerLoop >= 4.0)) {
+            if ((PositionRef2 < -0.40*(6.380)) && (TimerLoop >= 4.0)) {
                 CurrentOperationState = OPState3;
                 TimerLoop.reset();
             }
             break;
+            
         case OPState3:
-            LedGreen = 1; //Turqoise
-            if (PositionRef > -0.005*(6.380)) {
+            LedRed = 0; //pink
+            LedGreen = 1;
+            LedBlue = 0;
+            if (PositionRef > -0.15*(6.380)) {
                 PositionRef -= 0.0005*(6.380);
             }
-            if (PositionRef2 > -0.345*(6.380)) {
-                PositionRef2 -= 0.0005*(6.380);
+            if (PositionRef2 > -0.75*(6.380)) {
+                PositionRef2 -= 0.0002*(6.380);
             }
-            if ((PositionRef < -0.005*(6.380)) && (PositionRef2 < -0.345*(6.380)) && (TimerLoop >= 4.0)) {
-                
+            if ((PositionRef < -0.15*(6.380)) && (PositionRef2 < -0.75*(6.380)) && (TimerLoop >= 4.0)) {
                 CurrentOperationState = OPState4;
                 TimerLoop.reset();
             }
             break;
+            
         case OPState4:
-            LedGreen = 1; //Blue
-            if (PositionRef2 < 0.595*(6.380)) {
-                PositionRef2 += 0.0005*(6.380);
+            LedRed = 1; //blue
+            LedGreen = 1;
+            LedBlue = 0;
+            if (PositionRef2 > -1.4*(6.380)) {
+                PositionRef2 -= 0.005*(6.380);
             }
-            if (PositionRef2 > 0.595*(6.380)) {
+            if ((PositionRef2 < -1.4*(6.380)) && (TimerLoop > 4)) {
+                TimerLoop.reset();
                 CurrentOperationState = OPHoming;
             }
             break;
+            
         case OPHoming:
             LedGreen = 0; //Green
             LedBlue = 1;
             LedRed = 1;
+            if (PositionRef < 0) {
+                PositionRef += 0.003*(6.380);
+            }
+            if (PositionRef2 < 0) {
+                PositionRef2 += 0.001*(6.380);
+            }
+            if ((PositionRef > 0) && (PositionRef2 > 0) && (TimerLoop > 4)) {
+                CurrentOperationState = OPWait;
+            }
             break;
     }
 }
@@ -216,10 +253,10 @@
 //Ticker function
 void MeasureAndControl(void)
 {
-    
+
     //double PositionRef = GetReferencePosition();
     //double PositionRef2 = GetReferencePosition2();
-    
+
     OperationStateMachine();
     double PositionMotor = GetActualPosition();
     double PositionMotor2 = GetActualPosition2();