Diff: main.cpp

Revision:

29:a88dd750fdbd

Parent:

28:22530fdc149b

Child:

30:b304a7eb6908

diff -r 22530fdc149b -r a88dd750fdbd main.cpp
--- a/main.cpp	Thu Oct 01 14:53:18 2020 +0000
+++ b/main.cpp	Mon Nov 02 19:31:50 2020 +0000
@@ -221,16 +221,17 @@
                 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_DE*sin(th1)+l_OB*sin(th1)+l_AC*sin(th1+th2);
+                float yFoot = -l_DE*cos(th1)-l_OB*cos(th1)-l_AC*cos(th1+th2);
+                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;
@@ -273,12 +274,12 @@
                 vDesFoot[1]*=vMult;
                 
                 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
-                float xFoot_inv = -rDesFoot[0];
-                float yFoot_inv = rDesFoot[1];                
-                float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
+                float xFootd = -rDesFoot[0];
+                float yFootd = rDesFoot[1];                
+                float l_OE = sqrt( (pow(xFootd,2) + pow(yFootd,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(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
+                float th1_des = -((3.14159f/2.0f) + atan2(yFootd,xFootd) - 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] );
@@ -291,14 +292,31 @@
                 float de_y = 0;
         
                 // Calculate virtual force on foot
-                float fx = 0;
-                float fy = 0;
+                float xdelta = -xFootd - xFoot;
+                float ydelta = yFootd - yFoot;
+                float dydelta = vDesFoot[1] - dyFoot;
+                float dxdelta = vDesFoot[0] - dxFoot;
+                float fx = K_xx*xdelta+K_xy*ydelta+D_xx*dxdelta+D_xy*dydelta;
+                float fy = K_yy*ydelta+K_xy*xdelta+D_yy*dydelta+D_xy*dxdelta;
+                
+                float t1 = Jx_th1*fx + Jy_th1*fy;
+                float t2 = Jx_th2*fx + Jy_th2*fy;
                 
                 // 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 = pow(Ir*N,2)+m4*pow(l_AC,2)+m3*pow(l_A_m3,2)+m2*pow(l_B_m2,2)+I2+I3;
                 
+                float M11 = I1 + I2 + I3 + I4 + Ir + Ir*N*N + l_AC*l_AC*m4 + l_A_m3*l_A_m3*m3 + l_B_m2*l_B_m2*m2 + l_C_m4*l_C_m4*m4 + l_OA*l_OA*m3 + l_OB*l_OB*m2 + l_OA*l_OA*m4 + l_O_m1*l_O_m1*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 + l_AC*l_AC*m4 + l_A_m3*l_A_m3*m3 + l_B_m2*l_B_m2*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*N*N + m4*l_AC*l_AC + m3*l_A_m3*l_A_m3 + m2*l_B_m2*l_B_m2 + I2 + I3;
+ 
                 // Populate mass matrix
                 MassMatrix.Clear();
                 MassMatrix << M11 << M12
@@ -319,13 +337,19 @@
                 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 L22 = Lambda.getNumber(2,2);      
+//                float L11 = 1;
+//                float L12 = 0;
+//                float L21 = 0;
+//                float L22 = 1;
+                
+                // Calculate desired motor torqu
+                float t1_op = (Jx_th1*L11+Jy_th1*L21)*fx + (Jx_th1*L12+Jy_th1*L22)*fy;
+                float t2_op = (Jx_th2*L11+Jy_th2*L21)*fx + (Jx_th2*L12+Jy_th2*L22)*fy;      
                                 
                 // Set desired currents             
-                current_des1 = 0;          
-                current_des2 = 0;   
-
-
+                current_des1 = t1_op/k_t;          
+                current_des2 = t2_op/k_t;   
 
                 // Form output to send to MATLAB     
                 float output_data[NUM_OUTPUTS];