Revision 30:103aab609fd6, committed 2021-12-09

Comitter:

mayborne_

Date:

Thu Dec 09 23:56:47 2021 +0000

Parent:

29:8b4fd3d36882

Commit message:

test

Changed in this revision

diff -r 8b4fd3d36882 -r 103aab609fd6 main.cpp
--- a/main.cpp	Tue Oct 06 01:02:31 2020 +0000
+++ b/main.cpp	Thu Dec 09 23:56:47 2021 +0000
@@ -11,7 +11,7 @@
 
 #define BEZIER_ORDER_FOOT    7
 #define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
-#define NUM_OUTPUTS 19
+#define NUM_OUTPUTS 30
 
 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
 
@@ -221,16 +221,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 = 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 = .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;
@@ -285,19 +285,19 @@
                 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;
                 
                 // Calculate mass matrix elements
-                float M11 = 0; 
-                float M12 = 0; 
-                float M22 = 0;
+                float M11 = I1 + I2 + I3 + I4 + Ir + Ir*pow(N,2) + pow(l_AC,2)*m4 + pow(l_A_m3,2)*m3 + pow(l_B_m2,2)*m2 + pow(l_C_m4,2)*m4 + pow(l_OA,2)*m3 + pow(l_OB,2)*m2 + pow(l_OA,2)*m4 + pow(l_O_m1,2)*m1 + 2*l_C_m4*l_OA*m4 + 2*l_AC*l_C_m4*m4*cos(th2) + 2*l_AC*l_OA*m4*cos(th2) + 2*l_A_m3*l_OA*m3*cos(th2) + 2*l_B_m2*l_OB*m2*cos(th2); 
+                float M12 = I2 + I3 + pow(l_AC,2)*m4 + pow(l_A_m3,2)*m3 + pow(l_B_m2,2)*m2 + Ir*N + l_AC*l_C_m4*m4*cos(th2) + l_AC*l_OA*m4*cos(th2) + l_A_m3*l_OA*m3*cos(th2) + l_B_m2*l_OB*m2*cos(th2); 
+                float M22 = Ir*pow(N,2) + m4*pow(l_AC,2) + m3*pow(l_A_m3,2) + m2*pow(l_B_m2,2) + I2 + I3;
                 
                 
                 
@@ -314,22 +314,23 @@
                 // Once you have copied the elements of the mass matrix, uncomment the following section
                 
                 // Calculate Lambda matrix
-//                JacobianT = MatrixMath::Transpose(Jacobian);
-//                InverseMassMatrix = MatrixMath::Inv(MassMatrix);
-//                temp_product = Jacobian*InverseMassMatrix*JacobianT;
-//                Lambda = MatrixMath::Inv(temp_product); 
+                JacobianT = MatrixMath::Transpose(Jacobian);
+                InverseMassMatrix = MatrixMath::Inv(MassMatrix);
+                temp_product = Jacobian*InverseMassMatrix*JacobianT;
+                Lambda = MatrixMath::Inv(temp_product); 
                 
                 // Pull elements of Lambda matrix
-//                float L11 = Lambda.getNumber(1,1);
-//                float L12 = Lambda.getNumber(1,2);
-//                float L21 = Lambda.getNumber(2,1);
-//                float L22 = Lambda.getNumber(2,2);               
+                float L11 = Lambda.getNumber(1,1);
+                float L12 = Lambda.getNumber(1,2);
+                float L21 = Lambda.getNumber(2,1);
+                float L22 = Lambda.getNumber(2,2);               
                                 
                                 
                                 
-                // Set desired currents             
-                current_des1 = 0;          
-                current_des2 = 0;   
+                // Set desired currents   
+                float kb = .2;          
+                current_des1 = (fx*(Jx_th1*L11+Jy_th1*L21)+fy*(Jx_th1*L12+Jy_th1*L22))/kb;          
+                current_des2 = (fx*(Jx_th2*L11+Jy_th2*L21)+fy*(Jx_th2*L12+Jy_th2*L22))/kb;    
 
 
 
@@ -358,6 +359,18 @@
                 output_data[16] = rDesFoot[1];
                 output_data[17] = vDesFoot[0];
                 output_data[18] = vDesFoot[1];
+                output_data[19] = Jx_th1;
+                output_data[20] = Jx_th2;
+                output_data[21] = Jy_th1;
+                output_data[22] = Jy_th2;
+                output_data[23] = M11;
+                output_data[24] = M12;
+                output_data[25] = M22;
+                output_data[26] = L11;
+                output_data[27] = L12;
+                output_data[28] = L21;
+                output_data[29] = L22;
+                
                 
                 // Send data to MATLAB
                 server.sendData(output_data,NUM_OUTPUTS);