Morgan Mayborne
/
Bezier_Traj_Follower_Example
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: ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 17:1bb5aa45826e
- Parent:
- 16:f9ea2b2d410f
- Child:
- 18:21c8d94eddee
diff -r f9ea2b2d410f -r 1bb5aa45826e main.cpp
--- a/main.cpp Thu Sep 24 20:16:05 2020 +0000
+++ b/main.cpp Fri Sep 25 04:39:17 2020 +0000
@@ -7,9 +7,8 @@
#include "MotorShield.h"
#include "HardwareSetup.h"
-
#define BEZIER_ORDER_FOOT 7
-#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1) + 2*(BEZIER_ORDER_CURRENT+1) )
+#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
#define NUM_OUTPUTS 19
#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
@@ -73,16 +72,12 @@
float D_xx;
float D_xy;
float D_yy;
-float xFoot;
-float yFoot;
-float xDesFoot;
-float yDesFoot;
// Model parameters
-float supply_voltage = 12; // motor supply voltage
-float R; // motor resistance
-float k_emf; // motor torque constant
-float nu1, nu2; // motor viscous friction
+float supply_voltage = 12; // motor supply voltage
+float R = 2.5f; // motor resistance
+float k_t = 0.17f; // motor torque constant
+float nu = 0.0005; // motor viscous friction
// Current control interrupt function
void CurrentLoop()
@@ -132,6 +127,10 @@
int main (void)
{
+
+ // Object for 7th order Cartesian foot trajectory
+ BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
+
// Link the terminal with our server and start it up
server.attachTerminal(pc);
server.init();
@@ -146,7 +145,7 @@
if (server.getParams(input_params,NUM_INPUTS)) {
- // Get inputs from MATLAB
+ // Get inputs from MATLAB
start_period = input_params[0]; // First buffer time, before trajectory
traj_period = input_params[1]; // Trajectory time/length
end_period = input_params[2]; // Second buffer time, after trajectory
@@ -158,14 +157,14 @@
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?
float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
- foot_pts[i] = input_params[21+i];
+ foot_pts[i] = input_params[12+i];
}
rDesFoot_bez.setPoints(foot_pts);
@@ -186,7 +185,7 @@
// Run experiment
while( t.read() < start_period + traj_period + end_period) {
- // Read encoders to get motor states
+ // Read encoders to get motor states, multiply by negative to match defined generalized coordinates
angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
@@ -201,34 +200,34 @@
// 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 Jx_th2 = 0;
+// 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);
- // Calculate the forware kinematics (position and velocity)
- float xLeg = 0;
- float yLeg = 0;
- float dxLeg = 0;
- float fyLeg = 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 xLeg = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
-// float yLeg = - l_AC*cos(th1 + th2) - l_DE*cos(th1) - l_OB*cos(th1);
-// float dxLeg = Jx_th1 * dth1 + Jx_th2 * dth2;
-// float dyLeg = Jy_th1 * dth1 + Jy_th2 * dth2;
+ // 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;
// 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 = 180;
- K_yy = 180;
+ K_xx = 50;
+ K_yy = 50;
D_xx = 2;
D_yy = 2;
K_xy = 0;
@@ -249,8 +248,8 @@
}
else
{
- teff = traj_period;
- // TODO: reset gains and vMult here?
+ teff = traj_period;
+ vMult = 0;
}
float rDesFoot[2] , vDesFoot[2];
@@ -260,31 +259,54 @@
vDesFoot[1]/=traj_period;
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;
+
+ float l_OE = sqrt( (pow(xFoot,2) + pow(yFoot,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 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 = 0;
- float e_y = 0;
- float de_x = 0;
- float de_y = 0;
+// float e_x = 0;
+// float e_y = 0;
+// float de_x = 0;
+// float de_y = 0;
-// float e_x = ( xLeg - rDesFoot[0]);
-// float e_y = ( yLeg - rDesFoot[1]);
-// float de_x = ( dxLeg - vDesFoot[0]);
-// float de_y = ( dyLeg - vDesFoot[1]);
-//
+ float e_x = ( xFoot - rDesFoot[0]);
+ float e_y = ( yFoot - rDesFoot[1]);
+ float de_x = ( dxFoot - vDesFoot[0]);
+ float de_y = ( dyFoot - vDesFoot[1]);
+
// 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;
+ 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;
+// current_des1 = 0;
+// current_des2 = 0;
-// current_des1 = (nu1*velocity1 + Jx_th1*fx + Jy_th1*fy)/k_emf;
-// current_des2 = (nu2*velocity2 + Jx_th2*fx + Jy_th2*fy)/k_emf;
+ // 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;
//Form output to send to MATLAB
float output_data[NUM_OUTPUTS];
@@ -305,12 +327,12 @@
// foot state
output_data[11] = xFoot;
output_data[12] = yFoot;
- output_data[13]= dxFoot;
- output_data[14]= dyFoot;
- output_data[15]= rDesFoot[0];
- output_data[16]= rDesFoot[1];
- output_data[17]= vDesFoot[0];
- output_data[18]= vDesFoot[1];
+ output_data[13] = dxFoot;
+ output_data[14] = dyFoot;
+ output_data[15] = rDesFoot[0];
+ output_data[16] = rDesFoot[1];
+ output_data[17] = vDesFoot[0];
+ output_data[18] = vDesFoot[1];
// Send data to MATLAB
server.sendData(output_data,NUM_OUTPUTS);