Diff: main.cpp

Revision:

24:f6e9e29c9263

Parent:

23:80e05d939f8c

--- a/main.cpp	Mon Sep 28 20:54:51 2020 +0000
+++ b/main.cpp	Tue Nov 17 20:45:25 2020 +0000
@@ -6,6 +6,8 @@
 #include "BezierCurve.h"
 #include "MotorShield.h" 
 #include "HardwareSetup.h"
+#include <iostream>
+
 
 #define BEZIER_ORDER_FOOT    7
 #define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
@@ -179,7 +181,7 @@
             encoderC.reset();
             encoderD.reset();
 
-            motorShield.motorAWrite(0, 0); //turn motor A off
+            motorShield.motorAWrite(0, 0); //turn motor A off  
             motorShield.motorBWrite(0, 0); //turn motor B off
                          
             // Run experiment
@@ -198,28 +200,34 @@
                 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;
                 float vMult = 0;
                 if( t < start_period) {
                     if (K_xx > 0 || K_yy > 0) {
-                        K_xx = 50; // for joint space control, set this to 1
-                        K_yy = 50; // for joint space control, set this to 1
-                        D_xx = 2;  // for joint space control, set this to 0.1
-                        D_yy = 2;  // for joint space control, set this to 0.1
+                        K_xx = 1; // for joint space control, set this to 1
+                        K_yy = 1; // for joint space control, set this to 1
+                        D_xx = .1;  // for joint space control, set this to 0.1
+                        D_yy = .1;  // for joint space control, set this to 0.1
                         K_xy = 0;
                         D_xy = 0;
+                     //   K_xx = 40; // for joint space control, set this to 1
+//                        K_yy = 0; // for joint space control, set this to 1
+//                        D_xx = 0;  // for joint space control, set this to 0.1
+//                        D_yy = 10;  // for joint space control, set this to 0.1
+//                        K_xy = 0;
+//                        D_xy = 0;
                     }
                     teff = 0;
                 }
@@ -261,31 +269,36 @@
                 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 = xFootd-xFoot;
+                float e_y = yFootd-yFoot;
+                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);
+                
+                
                 // Set desired currents             
-                current_des1 = 0;          
-                current_des2 = 0;   
+                  
+               // current_des2 = 0;
+//                current_des1 = 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;                          
+               // current_des1 =(K_xx*(th1_des-th1)+D_xx*(dth1_des-dth1))/(k_t);
+//                current_des2 = (K_yy*(th2_des-th2)+D_yy*(dth2_des-dth2))/(k_t);                     
                            
                 // Cartesian impedance  
-                // Note: As with the joint space laws, be careful with signs!              
-//                current_des1 = 0;          
-//                current_des2 = 0;   
+                // Note: As with the joint space laws, be careful with signs!   
                 
+                current_des1 = Torque_q1/k_t;           
+                current_des2 = Torque_q2/k_t;  
                 
                 // Form output to send to MATLAB     
                 float output_data[NUM_OUTPUTS];