Diff: main.cpp

Revision:

26:5822d4d8dca7

Parent:

25:52b378e89f42

Child:

27:5d60c6ab6d0a

--- a/main.cpp	Tue Sep 29 21:09:51 2020 +0000
+++ b/main.cpp	Thu Oct 01 05:21:20 2020 +0000
@@ -6,6 +6,8 @@
 #include "BezierCurve.h"
 #include "MotorShield.h" 
 #include "HardwareSetup.h"
+#include "Matrix.h"
+#include "MatrixMath.h"
 
 #define BEZIER_ORDER_FOOT    7
 #define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
@@ -26,6 +28,13 @@
 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
 Ticker currentLoop;
 
+Matrix MassMatrix(2,2);
+Matrix Jacobian(2,2);
+Matrix JacobianT(2,2);
+Matrix InverseMassMatrix(2,2);
+Matrix temp_product(2,2);
+Matrix Lambda(2,2);
+
 // Variables for q1
 float current1;
 float current_des1 = 0;
@@ -51,6 +60,20 @@
 const float l_OB=.042; 
 const float l_AC=.096; 
 const float l_DE=.091;
+const float m1 =.0393 + .2;
+const float m2 =.0368; 
+const float m3 = .00783;
+const float m4 = .0155;
+const float I1 = 0.0000251;  //25.1 * 10^-6;
+const float I2 = 0.0000535;  //53.5 * 10^-6;
+const float I3 = 0.00000925; //9.25 * 10^-6;
+const float I4 = 0.0000222;  //22.176 * 10^-6;
+const float l_O_m1=0.032;
+const float l_B_m2=0.0344; 
+const float l_A_m3=0.0622;
+const float l_C_m4=0.0610;
+const float Nmot = 18.75;
+const float Ir = 0.0035/pow(Nmot,2);
 
 // Timing parameters
 float current_control_period_us = 200.0f;     // 5kHz current control loop
@@ -214,10 +237,10 @@
                 float vMult = 0;
                 if( t < start_period) {
                     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
+                        K_xx = 50; 
+                        K_yy = 50; 
+                        D_xx = 2;  
+                        D_yy = 2;  
                         K_xy = 0;
                         D_xy = 0;
                     }
@@ -270,22 +293,40 @@
                 // Calculate virtual force on foot
                 float fx = 0;
                 float fy = 0;
+                
+                // Calculate mass matrix elements
+                float M11 = I1 + I2 + I3 + I4 + Ir + Ir*pow(Nmot,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*Nmot + 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(Nmot,2) + m4*pow(l_AC,2) + m3*pow(l_A_m3,2) + m2*pow(l_B_m2,2) + I2 + I3;
+                
+                // Populate mass matrix
+                MassMatrix.Clear();
+                MassMatrix << M11 << M12
+                           << M12 << M22;
+                
+                // Populate Jacobian matrix
+                Jacobian.Clear();
+                Jacobian << Jx_th1 << Jx_th2
+                         << Jy_th1 << Jy_th2;
+                
+                // Calculate Lambda matrix
+                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);               
                                 
                 // Set desired currents             
                 current_des1 = 0;          
                 current_des2 = 0;   
-        
-                // 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;                          
-                           
-                // Cartesian impedance  
-                // Note: As with the joint space laws, be careful with signs!              
-//                current_des1 = 0;          
-//                current_des2 = 0;   
+
+
+
                 
                 
                 // Form output to send to MATLAB