Diff: StateMachinePTR.cpp

Revision:

9:3410f8dd6845

Parent:

8:428ff7b7715d

Child:

10:9c18b5d08c24

diff -r 428ff7b7715d -r 3410f8dd6845 StateMachinePTR.cpp
--- a/StateMachinePTR.cpp	Wed Oct 31 16:06:57 2018 +0000
+++ b/StateMachinePTR.cpp	Wed Oct 31 21:12:37 2018 +0000
@@ -89,6 +89,8 @@
 float VdesY; // Desired velocity in y direction
 float Error1; // Difference in reference angle and current angle motor 1
 float Error2; // Difference in reference angle and current angle motor 2
+int xDir;
+int yDir;
 
 //Print to screen
 int PrintFlag = false;
@@ -164,9 +166,11 @@
 // Function to set desired cartesian velocities of end-effector
 void Vdesired()
 {
-    double Vconst = 0.05; // m/s (5 cm per second)
-    VdesX = EMGBoolLeft*Vconst; // Left biceps is X-direction
-    VdesY = -1*EMGBoolRight*Vconst; // Right biceps is minus Y-direction
+    xDir = 1-BUT1.read();
+    yDir = 1-BUT2.read();
+    double Vconst = 0.03; // m/s (3 cm per second)
+    VdesX = xDir*Vconst; // Left biceps is X-direction
+    VdesY = -1*yDir*Vconst; // Right biceps is minus Y-direction
 }
 
 // Inverse Kinematics
@@ -185,8 +189,8 @@
     double q1DotRef = (InvJac11*VdesX + InvJac12*VdesY)*RatioGears; //reference angular velocity motor 1
     double q2DotRef = (InvJac21*VdesX + InvJac22*VdesY)*RatioPulleys; //reference angular velocity motor 2
 
-    q1Ref = q1 + q1DotRef*Ts; // set new reference position motor angle 1
-    q2Ref = q2 + q2DotRef*Ts; // set new reference position motor angle 1
+    q1Ref += q1DotRef*Ts; // set new reference position motor angle 1
+    q2Ref += q2DotRef*Ts; // set new reference position motor angle 1
 }
 
 //State machine
@@ -281,8 +285,8 @@
                     q1Ref -=0.0005*(6.2830);
                 }
                 if (BUTSW2 == false) {
-                    if (q1Ref<=0.733*(6.2830)) {
-                        q1Ref +=0.0005*(6.2830);
+                    if (q1Ref>=-0.52*(6.2830)) {
+                        q1Ref -=0.0005*(6.2830);
                     } else {
                         TimerLoop.reset();
                         NextMotorFlag = true;
@@ -303,14 +307,18 @@
                     q2Ref -= 0.0005*(6.2830);
                 }
                 if (BUTSW2 == false) {
-                    if (q2Ref>=-0.52*(6.2830)) {
-                        q2Ref -=0.0005*(6.2830);
+                    if (q2Ref<=0.733*(6.2830)) {
+                        q2Ref +=0.0005*(6.2830);
                     } else {
                         CurrentState = Homing;
                         Encoder1.reset();
                         Encoder2.reset();
                         q1Ref = 0;
                         q2Ref = 0;
+                        Error1 = 0;
+                        Error2 = 0;
+                        q1 = 0;
+                        q2 = 0;
                         TimerLoop.reset();
                     }
                 } // end of if (BUTSW2) statement
@@ -359,16 +367,19 @@
 
 void SetErrors()
 {
+    
     Error1 = q1Ref - q1;
     Error2 = q2Ref - q2;
+    
+    
 }
 //------------------------------------------------------------------------------
 // controller motor 1
 void PID_Controller1(double Err1)
 {
-    double Kp = 11.1; // proportional gain
-    double Ki = 2.24;//integral gain
-    double Kd = 1.1;  //derivative gain
+    double Kp = 19.8; // proportional gain
+    double Ki = 3.98;//integral gain
+    double Kd = 1.96;  //derivative gain
     static double ErrorIntegral = 0;
     static double ErrorPrevious = Err1;
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
@@ -393,8 +404,8 @@
 void PID_Controller2(double Err2)
 {
     double Kp = 11.1; // proportional gain
-    double Ki = 22.81;//integral gain
-    double Kd = 1.7;  //derivative gain
+    double Ki = 2.24;//integral gain
+    double Kd = 1.1;  //derivative gain
     static double ErrorIntegral = 0;
     static double ErrorPrevious = Err2;
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
@@ -486,7 +497,7 @@
 
     while (true) {
         if (PrintFlag == true) {
-            pc.printf("THleft = %f,THRight = %f,EMG left = %i, EMG right = %i, MOVAG Left = %f, MOVAG Right = %f\r",ThresholdLeft,ThresholdRight,EMGBoolLeft,EMGBoolRight,LeftBicepsOut, RightBicepsOut);
+            pc.printf("xButton = %i, yButton = %i, q1Ref = %f, q1 = %f, q2Ref = %f, q2= %f Error1 = %f, Error2 = %f\r",xDir,yDir,q1Ref,q1,q2Ref,q2,Error1,Error2);
         }
     }