Diff: main.cpp

Revision:

19:562c08086d71

Parent:

18:21c8d94eddee

Child:

21:74d660439219

diff -r 21c8d94eddee -r 562c08086d71 main.cpp
--- a/main.cpp	Fri Sep 25 15:11:05 2020 +0000
+++ b/main.cpp	Fri Sep 25 21:57:02 2020 +0000
@@ -62,9 +62,9 @@
 float start_period, traj_period, end_period;
 
 // Control parameters
-float current_Kp = 4.0f; //4.0f;         
-float current_Ki = 0.4f; //0.4f;           
-float current_int_max = 3.0f; //3.0f;       
+float current_Kp = 4.0f;         
+float current_Ki = 0.4f;           
+float current_int_max = 3.0f;       
 float duty_max;      
 float K_xx;
 float K_yy;
@@ -82,7 +82,7 @@
 // Current control interrupt function
 void CurrentLoop()
 {
-    // This loop sets the motor voltage commands using PI current controllers.
+    // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
     
     //use the motor shield as follows:
     //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
@@ -161,11 +161,11 @@
             K_yy                        = input_params[6];    // Foot stiffness N/m
             K_xy                        = input_params[7];    // Foot stiffness N/m
             D_xx                        = input_params[8];    // Foot damping N/(m/s)
-            D_yy                        = input_params[9];   // Foot damping N/(m/s)
+            D_yy                        = input_params[9];    // Foot damping N/(m/s)
             D_xy                        = input_params[10];   // Foot damping N/(m/s)
             duty_max                    = input_params[11];   // Maximum duty factor
           
-            // TODO: check that this gets inputs correctly?
+            // Get foot trajectory points
             float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
             for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
               foot_pts[i] = input_params[12+i];    
@@ -189,7 +189,7 @@
             // Run experiment
             while( t.read() < start_period + traj_period + end_period) { 
                  
-                // Read encoders to get motor states, multiply by negative to match defined generalized coordinates
+                // Read encoders to get motor states
                 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;       
                 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
                  
@@ -204,36 +204,26 @@
                 // ADD YOUR CONTROL CODE HERE (CALCULATE AND SET DESIRED CURRENTS)
  
                 // 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);
-                
+                float Jx_th1 = 0;
+                float Jx_th2 = 0;
+                float Jy_th1 = 0;
+                float Jy_th2 = 0;
+                                
                 // 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 = Jx_th1 * dth1 + Jx_th2 * dth2;
-                float dyFoot = Jy_th1 * dth1 + Jy_th2 * dth2;
+                float xFoot = 0;
+                float yFoot = 0;
+                float dxFoot = 0;
+                float dyFoot = 0;       
 
                 // 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 = 1.0f; //50; // TODO: for joint space control, set this to 1
-                        K_yy = 1.0f; //50; // for joint space control, set this to 1
-                        D_xx = 0.1f; //2;  // for joint space control, set this to 0.1
-                        D_yy = 0.1f; //2;  // for joint space control, set this to 0.1
+                        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_xy = 0;
                         D_xy = 0;
                     }
@@ -241,11 +231,11 @@
                 }
                 else if (t < start_period + traj_period)
                 {
-                    K_xx = input_params[5]; // Foot stiffness N/m
-                    K_yy = input_params[6]; // Foot stiffness N/m
-                    K_xy = input_params[7]; // Foot stiffness N/m
-                    D_xx = input_params[8]; // Foot damping N/(m/s)
-                    D_yy = input_params[9]; // Foot damping N/(m/s)
+                    K_xx = input_params[5];  // Foot stiffness N/m
+                    K_yy = input_params[6];  // Foot stiffness N/m
+                    K_xy = input_params[7];  // Foot stiffness N/m
+                    D_xx = input_params[8];  // Foot damping N/(m/s)
+                    D_yy = input_params[9];  // Foot damping N/(m/s)
                     D_xy = input_params[10]; // Foot damping N/(m/s)
                     teff = (t-start_period);
                     vMult = 1;
@@ -264,12 +254,7 @@
                 vDesFoot[0]*=vMult;
                 vDesFoot[1]*=vMult;
                 
-                // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
-//                float th1_des = 0;
-//                float th2_des = 0;
-//                float dth1_des = 0;
-//                float dth2_des = 0;
-                
+                // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
                 float xFootd = -rDesFoot[0];
                 float yFootd = rDesFoot[1];                
                 float l_OE = sqrt( (pow(xFootd,2) + pow(yFootd,2)) );
@@ -282,39 +267,30 @@
                 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 - rDesFoot[0]);
-                float e_y  = (  yFoot - rDesFoot[1]); 
-                float de_x = ( dxFoot - vDesFoot[0]);
-                float de_y = ( dyFoot - vDesFoot[1]);
+                float e_x = 0;
+                float e_y = 0;
+                float de_x = 0;
+                float de_y = 0;
         
                 // Calculate virtual force on foot
-//                float fx = 0;
-//                float fy = 0;
+                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_yy * e_y - K_xy * e_x - D_yy * de_y -D_xy * de_x;
-                
-                // Set desired currents
-//                current_des1 = 0;          
-//                current_des2 = 0;                
-                
+                // Set desired currents             
+                current_des1 = 0;          
+                current_des2 = 0;   
+        
                 // Joint impedance
                 // sub Kxx for K1, Dxx for D1, Kyy for K2, Dyy for D2
-//                current_des1 = (-K_xx*(th1) - D_xx*(dth1))/k_t;
-//                current_des2 = (-K_yy*(th2) - D_yy*(dth2))/k_t;
-                current_des1 = (-K_xx*(th1-th1_des) - D_xx*(dth1-dth1_des))/k_t;
-                current_des2 = (-K_yy*(th2-th2_des) - D_yy*(dth2-dth2_des))/k_t;
-                       
-                // Cartesian impedance         
-//                current_des1 = (Jx_th1*fx + Jy_th1*fy)/k_t;
-//                current_des2 = (Jx_th2*fx + Jy_th2*fy)/k_t;         
+//                current_des1 = 0;          
+//                current_des2 = 0;                          
+                           
+                // Cartesian impedance                
+//                current_des1 = 0;          
+//                current_des2 = 0;   
                 
-                //Form output to send to MATLAB     
+                
+                // Form output to send to MATLAB     
                 float output_data[NUM_OUTPUTS];
                 // current time
                 output_data[0] = t.read();