Sebastian Uribe
/
pan_flipping
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: MatrixMath Matrix ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 36:a3beb771d8f7
- Parent:
- 35:88dbfefc1bbb
- Child:
- 37:48a3017a958f
--- a/main.cpp Mon Nov 16 22:10:05 2020 +0000
+++ b/main.cpp Mon Nov 16 22:57:29 2020 +0000
@@ -53,10 +53,10 @@
float angle2_init;
// Fixed kinematic parameters
-const float l_OA=.011;
-const float l_OB=.042;
-const float l_AC=.096;
-const float l_DE=.091;
+//const float l_OA=.011;
+//const float l_OB=.042;
+//const float l_AC=.096;
+//const float l_DE=.091;
float l1; //=0.084125;
float l2; //=0.084125;
@@ -170,13 +170,13 @@
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
- l1 = input_params[12];
- l2 = input_params[13];
+ l1 = input_params[12]; // Length of 1st link
+ l2 = input_params[13]; // Length of 2nd link
// 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[14+i];
+ foot_pts[i] = input_params[14+i]; // Bezier curve points
}
rDesFoot_bez.setPoints(foot_pts);
@@ -236,22 +236,37 @@
// 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; // 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_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;
- }
+ if( t < start_period) { // impedance operational space to start point
teff = 0;
+ float rDesFoot[2] , vDesFoot[2];
+ rDesFoot[0] = l2;
+ rDesFoot[1] = -l1;
+ vDesFoot[0] = 0;
+ vDesFoot[1] =0;
+
+ // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
+ float xFootd = -rDesFoot[0];
+ float yFootd = rDesFoot[1];
+
+ // Calculate error variables
+ float e_x = xFootd-xFoot;
+ float e_y = yFootd-yFoot;
+ float de_x = vDesFoot[0]-dxFoot;
+ float de_y = vDesFoot[1]-dyFoot;
+
+ // Calculate virtual force on foot
+ 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 T1 = Jx_th1*fx+Jy_th1*fy;
+ float T2 = Jx_th2*fx+Jy_th2*fy;
+
+ // Cartesian impedance
+ // Note: As with the joint space laws, be careful with signs!
+ current_des1 = T1/k_t;
+ current_des2 = T2/k_t;
}
- else if (t < start_period + traj_period)
+ else if (t < start_period + traj_period) // torque control to follow Bezier
{
K_xx = input_params[5]; // Foot stiffness N/m
K_yy = input_params[6]; // Foot stiffness N/m
@@ -259,70 +274,46 @@
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;
- }
- else
- {
- teff = traj_period;
- vMult = 0;
+ float torque_des[2];
+ torque_bez.evaluate(teff/traj_period,torque_des);
+ current_des1 = torque_des[0]/k_t;
+ current_des2 = torque_des[1]/k_t;
}
-
- float rDesFoot[2] , vDesFoot[2];
- rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
- rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
- vDesFoot[0]/=traj_period;
- vDesFoot[1]/=traj_period;
- vDesFoot[0]*=vMult;
- vDesFoot[1]*=vMult;
-
- float torque_des[2];
- torque_bez.evaluate(teff/traj_period,torque_des);
-
- // 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)) );
- 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(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] );
- float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
-
- // Calculate error variables
- float e_x = xFootd-xFoot;
- float e_y = yFootd-yFoot;
- float de_x = vDesFoot[0]-dxFoot;
- float de_y = vDesFoot[1]-dyFoot;
-
- // Calculate virtual force on foot
- 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 T1 = Jx_th1*fx+Jy_th1*fy;
- float T2 = Jx_th2*fx+Jy_th2*fy;
+ else // impedance operational space to end point
+ {
+ teff = 0;
+ float rDesFoot[2] , vDesFoot[2];
+ rDesFoot[0] = l2;
+ rDesFoot[1] = -l1;
+ vDesFoot[0] = 0;
+ vDesFoot[1] =0;
-
- // Set desired currents
-
- // 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 = (K_xx*(th1_des-angle1) + D_xx*(dth1_des-velocity1))/k_t;
-// current_des2 = (K_yy*(th2_des-angle2) + D_yy*(dth2_des-velocity2))/k_t;
+ // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
+ float xFootd = -rDesFoot[0];
+ float yFootd = rDesFoot[1];
+
+ // Calculate error variables
+ float e_x = xFootd-xFoot;
+ float e_y = yFootd-yFoot;
+ float de_x = vDesFoot[0]-dxFoot;
+ float de_y = vDesFoot[1]-dyFoot;
+
+ // Calculate virtual force on foot
+ 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 T1 = Jx_th1*fx+Jy_th1*fy;
+ float T2 = Jx_th2*fx+Jy_th2*fy;
// Cartesian impedance
// Note: As with the joint space laws, be careful with signs!
- current_des1 = T1/k_t;
- current_des2 = T2/k_t;
-
- current_des1 = torque_des[0]/k_t;
- current_des2 = torque_des[1]/k_t;
-
-
+ current_des1 = T1/k_t;
+ current_des2 = T2/k_t;
+ }
+
// Form output to send to MATLAB
float output_data[NUM_OUTPUTS];
// current time
