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:
- 37:48a3017a958f
- Parent:
- 36:a3beb771d8f7
- Child:
- 38:8ce2f6edba26
diff -r a3beb771d8f7 -r 48a3017a958f main.cpp
--- a/main.cpp Mon Nov 16 22:57:29 2020 +0000
+++ b/main.cpp Mon Nov 16 23:25:57 2020 +0000
@@ -186,6 +186,7 @@
torque_pts[i] = input_params[14+i];
}
torque_bez.setPoints(torque_pts);
+ float torque_des[2];
// Attach current loop interrupt
currentLoop.attach_us(CurrentLoop,current_control_period_us);
@@ -237,32 +238,19 @@
float teff = 0;
float vMult = 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;
+
+ teff = 0.f;
+ float th1_d= 0.f;
+ float th2_d= -3.1415f/2.f;
+ float e_th1= th1_d-th1;
+ float e_th2= th2_d-th2;
- // 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 e_dth1= -dth1;
+ float e_dth2= -dth2;
- 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!
+ float T1 = K_xx*e_th1 + D_xx*e_dth1;
+ float T2 = K_yy*e_th2 + D_yy*e_dth2;
+
current_des1 = T1/k_t;
current_des2 = T2/k_t;
}
@@ -277,41 +265,55 @@
teff = (t-start_period);
vMult = 1;
- 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;
}
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;
+ teff = 0.f;
+ float th1_d= 0.f;
+ float th2_d= -3.1415f/2.f;
+ float e_th1= th1_d-th1;
+ float e_th2= th2_d-th2;
- // 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 e_dth1= -dth1;
+ float e_dth2= -dth2;
- 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!
+ float T1 = K_xx*e_th1 + D_xx*e_dth1;
+ float T2 = K_yy*e_th2 + D_yy*e_dth2;
+
current_des1 = T1/k_t;
current_des2 = T2/k_t;
+
+// 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;
}
// Form output to send to MATLAB
