Diff: StateMachinePTR.cpp

Revision:

11:2ffae0f2110a

Parent:

10:9c18b5d08c24

Child:

12:d19588a50fc7

--- a/StateMachinePTR.cpp	Wed Oct 31 21:40:50 2018 +0000
+++ b/StateMachinePTR.cpp	Thu Nov 01 09:08:09 2018 +0000
@@ -93,7 +93,8 @@
 float Error2; // Difference in reference angle and current angle motor 2
 int xDir;
 int yDir;
-
+float RatioGears = 134.0/30.0;
+float RatioPulleys = 87.4/27.5;
 //Print to screen
 int PrintFlag = false;
 int CounterPrint = 0;
@@ -161,9 +162,6 @@
     q1 = Counts1/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2)
     q2 = Counts2/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2)
     
-    //Gear Ratio's
-    double RatioGears = 134.0/30.0; //Gear Ratio for motor 1
-    double RatioPulleys = 87.4/27.5; // Gear Ratio for motor 2
     
     qArm = -1*q1/RatioGears; //Adjust for opposite direction due to gears
     qEndEff = q2/RatioPulleys; 
@@ -177,7 +175,7 @@
 {
     xDir = 1-BUT1.read();
     yDir = 1-BUT2.read();
-    double Vconst = 0.03; // m/s (3 cm per second)
+    double Vconst = 0.05; // m/s (3 cm per second)
     VdesX = xDir*Vconst; // Left biceps is X-direction
     VdesY = -1*yDir*Vconst; // Right biceps is minus Y-direction
 }
@@ -231,7 +229,9 @@
             LedGreen = 0;
             LedBlue = 0; //White
             static bool Switch2Demo = false;
-            if (BUT2 == false) {
+            static bool Switch2OP = false;
+            if (BUT2 == false) 
+            {
                 Switch2Demo = true;
                 TimerLoop.reset();
             }
@@ -240,11 +240,18 @@
                 CurrentState = Demo;
                 Switch2Demo = false;   
             }
-
-            if (BUT1 == false) {
-                CurrentState = Operation;
+            if (BUT1 == false) 
+            {
+                Switch2OP = true;
                 TimerLoop.reset();
             }
+            if ((Switch2OP == true) && (TimerLoop.read()>=2.0))
+            {
+                CurrentState = Operation;
+                Switch2OP = false;   
+            }
+            
+            
             break;
 
         case Emergency:
@@ -382,8 +389,8 @@
 {
     if (CurrentState == Demo)
     {
-    Error1 = q1Ref - qArm;
-    Error2 = q2Ref - qEndEff;
+    Error1 = -1*RatioGears*(q1Ref - qArm);
+    Error2 = RatioPulleys*(q2Ref - qEndEff);
     }
     else
     Error1 = q1Ref - q1;