Sebastian Uribe
/
pan_flipping
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: MatrixMath Matrix ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 29:a88dd750fdbd
- Parent:
- 28:22530fdc149b
- Child:
- 30:b304a7eb6908
--- 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];
