Revision 26:93506abcf763, committed 2021-12-09

Comitter:

mayborne_

Date:

Thu Dec 09 23:55:33 2021 +0000

Parent:

25:52b378e89f42

Commit message:

test

Changed in this revision

--- a/main.cpp	Tue Sep 29 21:09:51 2020 +0000
+++ b/main.cpp	Thu Dec 09 23:55:33 2021 +0000
@@ -198,16 +198,16 @@
                 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 = .096*cos(th1+th2) + .091*cos(th1) + .042*cos(th1);
+                float Jx_th2 = .096*cos(th1+th2);
+                float Jy_th1 = .096*sin(th1+th2)+ .091*sin(th1) + .042*sin(th1);
+                float Jy_th2 = .096*sin(th1+th2);
                                 
                 // Calculate the forward kinematics (position and velocity)
-                float xFoot = 0;
-                float yFoot = 0;
-                float dxFoot = 0;
-                float dyFoot = 0;       
+                float xFoot = .096*sin(th1+th2) + .091*sin(th1) + .042*sin(th1);
+                float yFoot = -.096*cos(th1+th2) - .091*cos(th1) - .042*cos(th1);
+                float dxFoot = Jx_th1*dth1 + Jx_th2*dth2;
+                float dyFoot = Jy_th1*dth1 + Jy_th2*dth2;       
 
                 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
                 float teff  = 0;
@@ -216,8 +216,8 @@
                     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
+                        D_xx = .1;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
+                        D_yy = .1;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
                         K_xy = 0;
                         D_xy = 0;
                     }
@@ -262,30 +262,39 @@
                 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;
+                //float pi = 3.14159;
+                //float th = atan(yFoot/xFoot) - pi/4;
+                //float fx = e_x*(cos(th)*(K_xx*cos(th) + K_xy*sin(th)) + sin(th)*(K_xy*cos(th) + K_yy*sin(th))) + e_y*(cos(th)*(K_xy*cos(th) + K_yy*sin(th)) - sin(th)*(K_xx*cos(th) + K_xy*sin(th)));
+                //float fy = e_x*(cos(th)*(K_xy*cos(th) - K_xx*sin(th)) + sin(th)*(K_yy*cos(th) - K_xy*sin(th))) + e_y*(cos(th)*(K_yy*cos(th) - K_xy*sin(th)) - sin(th)*(K_xy*cos(th) - K_xx*sin(th)));
                                 
-                // Set desired currents             
-                current_des1 = 0;          
-                current_des2 = 0;   
+                // Set desired currents 
+                float K1 = 1.0;
+                float D1 = 0.05;
+                float K2 = 1.0;
+                float D2 = 0.05;  
+                float kb = .2;           
+                //current_des1 =  (-K1*angle1-D1*velocity1)/kb;          
+                //current_des2 = (-K2*angle2-D2*velocity2)/kb;    
         
                 // Joint impedance
                 // 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;                          
+                //current_des1 = K_xx*(th1_des - th1) + D_xx*(dth1_des - dth1);          
+                //current_des2 = K_yy*(th2_des - th2) + D_yy*(dth2_des - dth2);                          
                            
                 // Cartesian impedance  
                 // Note: As with the joint space laws, be careful with signs!              
-//                current_des1 = 0;          
-//                current_des2 = 0;   
+                current_des1 = (fx*Jx_th1+fy*Jy_th1)/kb;          
+                current_des2 = (fx*Jx_th2+fy*Jy_th2)/kb;   
                 
                 
                 // Form output to send to MATLAB