Revision 16:1c8a8643260f, committed 2018-11-01

Comitter:

Roooos

Date:

Thu Nov 01 10:10:54 2018 +0000

Parent:

15:97311058e012

Commit message:

aangepast op StateMachinePTR, werkende en met Homing

Changed in this revision

diff -r 97311058e012 -r 1c8a8643260f OperationMode.cpp
--- a/OperationMode.cpp	Thu Nov 01 09:38:25 2018 +0000
+++ b/OperationMode.cpp	Thu Nov 01 10:10:54 2018 +0000
@@ -25,40 +25,47 @@
 DigitalOut LedRed(LED_RED);
 DigitalOut LedBlue(LED_BLUE);
 
-double PositionRef=0;
+double q1Ref=0;
 double q2Ref =0;
 
 
+
+
 //Define substates Operation Mode
 
 enum States {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming};
 States CurrentOperationState;
 
+void SetMotorsOff()
+{
+    PwmPin = 0;
+    PwmPin2 = 0;
+}
 //State Machine for OperationMode
 void OperationStateMachine()
 {
     switch (CurrentOperationState) {
         case OPWait:
-            if (EMGBoolLeft == true) {    //When Left Biceps are contracted, initiate chain to flip the page
+            if (BUT1 == false) {    //When Left Biceps are contracted, initiate chain to flip the page
                 CurrentOperationState = OPState1;
                 TimerLoop.reset();
             }
-            if (BUTSW3 == false) {  //When sw3 is pressed, go to homing for Operation Mode
+            if (BUT2 == false) {  //When sw3 is pressed, go to homing for Operation Mode
                 CurrentOperationState = OPSet;
                 TimerLoop.reset();
             }
             break;
-            
+
         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 (q2Ref  < -0.733*(6.380) && TimerLoop > 2) {
                 CurrentOperationState = OPWait;
                 TimerLoop.reset();
-                }
+            }
             break;
-                
+
         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);
@@ -68,7 +75,7 @@
                 TimerLoop.reset();
             }
             break;
-            
+
         case OPState2:                      //Second step of chain to flip the page
             if (q2Ref  > -1.133*(6.380)) {
                 q2Ref  -= 0.0005*(6.380);
@@ -78,7 +85,7 @@
                 TimerLoop.reset();
             }
             break;
-            
+
         case OPState3:                  //Third step of chain to flip the page
             if (q1Ref > -0.15*(6.380)) {
                 q1Ref -= 0.0005*(6.380);
@@ -91,7 +98,7 @@
                 TimerLoop.reset();
             }
             break;
-            
+
         case OPState4:                          //Fourth step of chain to flip the page
             if (q2Ref  > -2.133*(6.380)) {
                 q2Ref  -= 0.005*(6.380);
@@ -101,7 +108,7 @@
                 CurrentOperationState = OPHoming;
             }
             break;
-            
+
         case OPHoming:                          //Go back to Home for Operation Mode and back to Waiting
             if (q1Ref < 0) {
                 q1Ref += 0.003*(6.380);
@@ -115,9 +122,24 @@
             break;
     }
 }
-
-
-
+//Homing 
+/*
+if (q1Ref >= 0) {
+                q1Ref -= 0.001*(6.380);
+            }
+            if (q1Ref <= 0) {
+                q1Ref += 0.001*(6.380);
+            }
+            if (q2Ref >= 0) {
+                q2Ref -= 0.001*(6.380);
+            }
+            if (q2Ref <= 0) {
+                q2Ref += 0.001*(6.380);
+            }
+            if ((-0.01*(6.380) < q1Ref < 0.01*(6.380)) && (-0.01*(6.380) < q1Ref < 0.01*(6.380))) {
+                SetMotorsOff();
+            }
+*/
 
 //-----------------------------------------------------------------------------
 //The double-functions