Diff: main.cpp

Revision:

24:26a515ebb7cf

Parent:

23:80e05d939f8c

Child:

25:52b378e89f42

diff -r 80e05d939f8c -r 26a515ebb7cf main.cpp
--- a/main.cpp	Mon Sep 28 20:54:51 2020 +0000
+++ b/main.cpp	Tue Sep 29 20:38:35 2020 +0000
@@ -214,10 +214,10 @@
                 float vMult = 0;
                 if( t < start_period) {
                     if (K_xx > 0 || K_yy > 0) {
-                        K_xx = 50; // for joint space control, set this to 1
-                        K_yy = 50; // for joint space control, set this to 1
-                        D_xx = 2;  // for joint space control, set this to 0.1
-                        D_yy = 2;  // for joint space control, set this to 0.1
+                        K_xx = 1; // for joint space control, set this to 50
+                        K_yy = 1; // for joint space control, set this to 50
+                        D_xx = 0.1;  // for joint space control, set this to 2
+                        D_yy = 0.1;  // for joint space control, set this to 2
                         K_xy = 0;
                         D_xy = 0;
                     }
@@ -240,6 +240,7 @@
                     vMult = 0;
                 }
                 
+                // Get desired foot positions and velocities
                 float rDesFoot[2] , vDesFoot[2];
                 rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
                 rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
@@ -249,12 +250,12 @@
                 vDesFoot[1]*=vMult;
                 
                 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
-                float xFootd = -rDesFoot[0];
-                float yFootd = rDesFoot[1];                
-                float l_OE = sqrt( (pow(xFootd,2) + pow(yFootd,2)) );
+                float xFoot_inv = -rDesFoot[0];
+                float yFoot_inv = rDesFoot[1];                
+                float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
                 float alpha = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
                 float th2_des = -(3.14159f - alpha); 
-                float th1_des = -((3.14159f/2.0f) + atan2(yFootd,xFootd) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
+                float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
                 
                 float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
                 float dth1_des = (1.0f/dd) * (  Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );