B M
first
Dependencies: ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 19:562c08086d71
- Parent:
- 18:21c8d94eddee
- Child:
- 21:74d660439219
--- 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();