Diff: main.cpp

Revision:

18:21c8d94eddee

Parent:

17:1bb5aa45826e

Child:

19:562c08086d71

--- a/main.cpp	Fri Sep 25 04:39:17 2020 +0000
+++ b/main.cpp	Fri Sep 25 15:11:05 2020 +0000
@@ -62,9 +62,9 @@
 float start_period, traj_period, end_period;
 
 // Control parameters
-float current_Kp = 0.7;           
-float current_Ki = 0.2;           
-float current_int_max = 1.0;      
+float current_Kp = 4.0f; //4.0f;         
+float current_Ki = 0.4f; //0.4f;           
+float current_int_max = 3.0f; //3.0f;       
 float duty_max;      
 float K_xx;
 float K_yy;
@@ -75,8 +75,8 @@
 
 // Model parameters
 float supply_voltage = 12;     // motor supply voltage
-float R = 2.5f;                // motor resistance
-float k_t = 0.17f;             // motor torque constant
+float R = 2.0f;                // motor resistance
+float k_t = 0.18f;             // motor torque constant
 float nu = 0.0005;             // motor viscous friction
 
 // Current control interrupt function
@@ -87,11 +87,13 @@
     //use the motor shield as follows:
     //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
         
-    current1     = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);   // measure current
-    float err_c1 = current_des1 - current1;                                         // current errror
-    current_int1 += err_c1;                                                         // integrate error
-    current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max);  // anti-windup
-    duty_cycle1 = current_Kp*err_c1 + current_Ki*current_int1;                      // PI current controller
+    current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
+    velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
+    float err_c1 = current_des1 - current1;                                             // current errror
+    current_int1 += err_c1;                                                             // integrate error
+    current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max);      // anti-windup
+    float ff1 = R*current_des1 + k_t*velocity1;                                         // feedforward terms
+    duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage;   // PI current controller
     
     float absDuty1 = abs(duty_cycle1);
     if (absDuty1 > duty_max) {
@@ -105,11 +107,13 @@
     }             
     prev_current_des1 = current_des1; 
     
-    current2     = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f);   // measure current
-    float err_c2 = current_des2 - current2;                                         // current error
-    current_int2 += err_c2;                                                         // integrate error
-    current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max);  // anti-windup   
-    duty_cycle2 = current_Kp*err_c2 + current_Ki*current_int2;                      // PI current controller
+    current2     = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f);       // measure current
+    velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;                                  // measure velocity  
+    float err_c2 = current_des2 - current2;                                             // current error
+    current_int2 += err_c2;                                                             // integrate error
+    current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max);      // anti-windup   
+    float ff2 = R*current_des2 + k_t*velocity2;                                         // feedforward terms
+    duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage;   // PI current controller
     
     float absDuty2 = abs(duty_cycle2);
     if (absDuty2 > duty_max) {
@@ -226,10 +230,10 @@
                 float vMult = 0;
                 if( t < start_period) {
                     if (K_xx > 0 || K_yy > 0) {
-                        K_xx = 50;
-                        K_yy = 50;
-                        D_xx = 2;
-                        D_yy = 2;
+                        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_xy = 0;
                         D_xy = 0;
                     }
@@ -266,10 +270,12 @@
 //                float dth1_des = 0;
 //                float dth2_des = 0;
                 
-                float l_OE = sqrt( (pow(xFoot,2) + pow(yFoot,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,xFoot) - abs(asin( (l_AC/l_OE)*sin(alpha) ));
+                float th2_des = -(3.14159f - 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] );
@@ -289,7 +295,7 @@
                 // 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_yy * e_y - K_xy * e_x - D_yy * de_y -D_xy * de_x;