Diff: main.cpp

Revision:

26:3f77fc38bcf4

Parent:

25:52b378e89f42

--- a/main.cpp	Tue Sep 29 21:09:51 2020 +0000
+++ b/main.cpp	Wed Dec 08 06:19:44 2021 +0000
@@ -26,6 +26,14 @@
 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
 Ticker currentLoop;
 
+float a;
+float b;
+float c;
+float d;
+float t1;
+float t2;
+
+
 // Variables for q1
 float current1;
 float current_des1 = 0;
@@ -198,26 +206,26 @@
                 const float dth2= velocity2;
  
                 // Calculate the Jacobian
-                float Jx_th1 = 0;
-                float Jx_th2 = 0;
-                float Jy_th1 = 0;
-                float Jy_th2 = 0;
+                float Jx_th1 = l_AC*cos(th1 + th2) + l_DE*cos(th1) + l_OB*cos(th1);
+                float Jx_th2 = l_AC*cos(th1 + th2);
+                float Jy_th1 = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
+                float Jy_th2 = l_AC*sin(th1 + th2);
                                 
                 // Calculate the forward kinematics (position and velocity)
-                float xFoot = 0;
-                float yFoot = 0;
-                float dxFoot = 0;
-                float dyFoot = 0;       
+                float xFoot = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
+                float yFoot =  -l_AC*cos(th1 + th2) - l_DE*cos(th1) - l_OB*cos(th1);
+                float dxFoot = dth1*(l_AC*cos(th1 + th2) + l_DE*cos(th1) + l_OB*cos(th1)) + dth2*l_AC*cos(th1 + th2);
+                float dyFoot = dth1*(l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1)) + dth2*l_AC*sin(th1 + th2);       
 
                 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
                 float teff  = 0;
                 float vMult = 0;
                 if( t < start_period) {
                     if (K_xx > 0 || K_yy > 0) {
-                        K_xx = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
-                        K_yy = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
-                        D_xx = 0.1;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
-                        D_yy = 0.1;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
+                        K_xx = 50; // for joint space control, set this to 1; for Cartesian space control, set this to 50
+                        K_yy = 50; // for joint space control, set this to 1; for Cartesian space control, set this to 50
+                        D_xx = 2;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
+                        D_yy = 2;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
                         K_xy = 0;
                         D_xy = 0;
                     }
@@ -262,14 +270,21 @@
                 float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
         
                 // Calculate error variables
-                float e_x = 0;
-                float e_y = 0;
-                float de_x = 0;
-                float de_y = 0;
+                float e_x = rDesFoot[0]-xFoot;
+                float e_y = rDesFoot[1]-yFoot;
+                float de_x = vDesFoot[0]-dxFoot;
+                float de_y = vDesFoot[1]-dyFoot;
         
                 // Calculate virtual force on foot
-                float fx = 0;
-                float fy = 0;
+                float fx = K_xx*e_x+K_xy*e_y+D_xx*de_x+D_xy*de_y;
+                float fy = K_xy*e_x+K_yy*e_y+D_xy*de_x+D_yy*de_y;
+                a = Jx_th1;
+                b = Jy_th1;
+                c = Jx_th2;
+                d = Jy_th2;
+                
+                t1 = a*fx+b*fy;
+                t2 = c*fx+d*fy;
                                 
                 // Set desired currents             
                 current_des1 = 0;          
@@ -279,13 +294,16 @@
                 // sub Kxx for K1, Dxx for D1, Kyy for K2, Dyy for D2
                 // Note: Be careful with signs now that you have non-zero desired angles!
                 // Your equations should be of the form i_d = K1*(q1_d - q1) + D1*(dq1_d - dq1)
-//                current_des1 = 0;          
-//                current_des2 = 0;                          
+                //(-K_xx*angle1-D_xx*velocity1)/k_t
+ //               current_des1 = (K_xx*(th1_des - angle1)+D_xx*(dth1_des - velocity1))/k_t;          
+//                current_des2 = (K_yy*(th2_des - angle2)+D_yy*(dth2_des - velocity2))/k_t; 
+                //current_des1 = 0;          
+                //current_des2 = 0;                         
                            
                 // Cartesian impedance  
                 // Note: As with the joint space laws, be careful with signs!              
-//                current_des1 = 0;          
-//                current_des2 = 0;   
+                current_des1 = t1/k_t;          
+                current_des2 = t2/k_t;   
                 
                 
                 // Form output to send to MATLAB