B M
final
Dependencies: MatrixMath Matrix ExperimentServer QEI_pmw MotorShield
Revision 30:833008a20edd, committed 2020-11-17
- Comitter:
- benj1man3
- Date:
- Tue Nov 17 20:21:23 2020 +0000
- Parent:
- 29:8b4fd3d36882
- Commit message:
- first one;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 8b4fd3d36882 -r 833008a20edd main.cpp
--- a/main.cpp Tue Oct 06 01:02:31 2020 +0000
+++ b/main.cpp Tue Nov 17 20:21:23 2020 +0000
@@ -204,7 +204,18 @@
motorShield.motorAWrite(0, 0); //turn motor A off
motorShield.motorBWrite(0, 0); //turn motor B off
-
+
+ float M11=0;
+ float M12=0;
+ float M22=0;
+ float Jx_th1=0;
+ float Jx_th2=0;
+ float Jy_th1=0;
+ float Jy_th2=0;
+ float L11=0;
+ float L12=0;
+ float L21=0;
+ float L22=0;
// Run experiment
while( t.read() < start_period + traj_period + end_period) {
@@ -221,16 +232,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 = 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;
@@ -285,20 +296,27 @@
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 = -xFoot_inv-xFoot;
+ float e_y = yFoot_inv-yFoot;
+// float de_x = vDesFoot[0]-dxFoot;
+// float de_y = vDesFoot[1]-dyFoot;
+ float de_x = -dxFoot;
+ float de_y = -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 Torque_q1= (Jx_th1*fx+Jy_th1*fy);
+// float Torque_q2= (Jx_th2*fx+Jy_th2*fy);
// Calculate mass matrix elements
- float M11 = 0;
- float M12 = 0;
- float M22 = 0;
-
+ 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);
+ // I1 + I2 + I3 + I4 + Ir + Ir*N^2 + l_AC^2*m4 + l_A_m3^2*m3 + l_B_m2^2*m2 + l_C_m4^2*m4 + l_OA^2*m3 + l_OB^2*m2 + l_OA^2*m4 + 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)
+ 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);
+ // I2 + I3 + l_AC^2*m4 + l_A_m3^2*m3 + 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)
+ M22 = Ir*pow(N,2) + m4*pow(l_AC,2) + m3*pow(l_A_m3,2) + m2*pow(l_B_m2,2) + I2 + I3;
+ // Ir*N^2 + m4*l_AC^2 + m3*l_A_m3^2 + m2*l_B_m2^2 + I2 + I3
// Populate mass matrix
@@ -314,22 +332,24 @@
// 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);
+ L11 = Lambda.getNumber(1,1);
+ L12 = Lambda.getNumber(1,2);
+ L21 = Lambda.getNumber(2,1);
+ L22 = Lambda.getNumber(2,2);
+
-
-
+ float Torque_q1= (Jx_th1*L11+Jy_th1*L12)*fx+(Jx_th1*L12+Jy_th1*L22)*fy;
+ float Torque_q2= (Jx_th2*L11+Jy_th2*L12)*fx+(Jx_th2*L12+Jy_th2*L22)*fy;
+
// Set desired currents
- current_des1 = 0;
- current_des2 = 0;
+ current_des1 = Torque_q1/k_t;
+ current_des2 = Torque_q2/k_t;
@@ -370,10 +390,21 @@
currentLoop.detach();
motorShield.motorAWrite(0, 0); //turn motor A off
motorShield.motorBWrite(0, 0); //turn motor B off
+ pc.printf("\n M11: %f", M11);
+ pc.printf("\n M12: %f", M12);
+ pc.printf("\n M22: %f", M22);
+ pc.printf("\n Jx_th1: %f", Jx_th1);
+ pc.printf("\n Jx_th2: %f", Jx_th2);
+ pc.printf("\n Jy_th1: %f", Jy_th1);
+ pc.printf("\n Jy_th2: %f", Jy_th2);
+ pc.printf("\n L11: %f", L11);
+ pc.printf("\n L12: %f", L12);
+ pc.printf("\n L21: %f", L21);
+ pc.printf("\n L22: %f", L22);
} // end if
} // end while
-
+// using namespace std;
} // end main