Laura Schwendeman
als;djfpoafb hnw3jg
Dependencies: MatrixMath Matrix ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 5:3d30627ae76e
- Parent:
- 4:bb441c9325f4
- Child:
- 6:7f39aa2c97da
--- a/main.cpp Mon Nov 29 20:49:05 2021 +0000
+++ b/main.cpp Tue Nov 30 21:52:04 2021 +0000
@@ -10,8 +10,8 @@
#include "HardwareSetup.h"
#define BEZIER_ORDER_FOOT 7
-#define NUM_INPUTS (21 + 2*(BEZIER_ORDER_FOOT+1))
-#define NUM_OUTPUTS 29
+#define NUM_INPUTS (27)
+#define NUM_OUTPUTS 33
#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
@@ -112,6 +112,14 @@
float k_t = 0.18f; // motor torque constant
float nu = 0.0005; // motor viscous friction
+// ellipse stuff
+float y0;
+float x0;
+float Omega;
+float a;
+float b;
+float phase;
+
// Current control interrupt function
void CurrentLoop()
{
@@ -211,7 +219,7 @@
// Object for 7th order Cartesian foot trajectory.
//CREATE A TRAJECTORY
- BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
+ //BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
// Link the terminal with our server and start it up
server.attachTerminal(pc);
@@ -253,13 +261,21 @@
D_yy2 = input_params[18];
D_xy2 = input_params[19];
duty_max2 = input_params[20];
+
+ a = input_params[21];
+ b = input_params[22];
+ Omega = input_params[23];
+ y0 = input_params[24];
+ x0 = input_params[25];
+ phase = input_params[26];
+
// 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[21+i];
- }
- rDesFoot_bez.setPoints(foot_pts);
+ //float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
+// for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
+// foot_pts[i] = input_params[27+i];
+// }
+// rDesFoot_bez.setPoints(foot_pts);
// Attach current loop interrupt
currentLoop.attach_us(CurrentLoop,current_control_period_us);
@@ -301,10 +317,10 @@
const float dth2= velocity2;
- const float th3 = angle3;
- const float th4 = angle4;
- const float dth3= velocity3;
- const float dth4= velocity4;
+ const float th3 = -angle3;
+ const float th4 = -angle4;
+ const float dth3= -velocity3;
+ const float dth4= -velocity4;
@@ -384,73 +400,85 @@
}
// Get desired foot positions and velocities
- 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 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 rDesFoot2[2] , vDesFoot2[2];
- float evalPoint = teff/traj_period+traj_period/2;
- if(evalPoint>traj_period) evalPoint=evalPoint-traj_period;
- rDesFoot_bez.evaluate(evalPoint,rDesFoot2);
- rDesFoot_bez.evaluateDerivative(evalPoint,vDesFoot2);
- vDesFoot2[0]/=traj_period;
- vDesFoot2[1]/=traj_period;
- vDesFoot2[0]*=vMult;
- vDesFoot2[1]*=vMult;
+ // float rDesFoot2[2] , vDesFoot2[2];
+// float evalPoint = teff/traj_period+traj_period/2;
+// if(evalPoint>traj_period) evalPoint=evalPoint-traj_period;
+// rDesFoot_bez.evaluate(evalPoint,rDesFoot2);
+// rDesFoot_bez.evaluateDerivative(evalPoint,vDesFoot2);
+// vDesFoot2[0]/=traj_period;
+// vDesFoot2[1]/=traj_period;
+// vDesFoot2[0]*=vMult;
+// vDesFoot2[1]*=vMult;
// Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
- float xFoot_inv = -rDesFoot[0];
- float yFoot_inv = rDesFoot[1];
- float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,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_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
-
-
- float xFoot_inv2 = -rDesFoot2[0];
- float yFoot_inv2 = rDesFoot2[1];
- float alpha2 = 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 th3_des = -(3.14159f - alpha2);
- float th4_des = -((3.14159f/2.0f) + atan2(yFoot_inv2,xFoot_inv2) - abs(asin( (l_AC/l_OE)*sin(alpha2) )));
-
-
+ //float xFoot_inv = -rDesFoot[0];
+// float yFoot_inv = rDesFoot[1];
+// float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,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_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
+//
+//
+// float xFoot_inv2 = -rDesFoot2[0];
+// float yFoot_inv2 = rDesFoot2[1];
+// float alpha2 = 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 th3_des = -(3.14159f - alpha2);
+// float th4_des = -((3.14159f/2.0f) + atan2(yFoot_inv2,xFoot_inv2) - abs(asin( (l_AC/l_OE)*sin(alpha2) )));
+//
+//
+//
+// 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] );
+//
+// float dd2 = (Jx_th3*Jy_th4 - Jx_th4*Jy_th3);
+// float dth3_des = (1.0f/dd2) * ( Jy_th4*vDesFoot[0] - Jx_th4*vDesFoot[1] );
+// float dth4_des = (1.0f/dd2) * ( -Jy_th3*vDesFoot[0] + Jx_th3*vDesFoot[1] );
+
+ // Calculate error variables and the desired position in ellispe
+ float xDes = b*cos(Omega*teff) + x0;
+ float yDes = a*sin(Omega*teff) + y0;
+ float vxDes = 0;//-b*Omega*sin(Omega*teff);
+ float vyDes = 0; //a*Omega*cos(Omega*teff);
- 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] );
+ float e_x = xDes - xFoot;
+ float e_y = yDes - yFoot;
+ float de_x = vxDes - dxFoot;
+ float de_y = vyDes - dyFoot;
- float dd2 = (Jx_th3*Jy_th4 - Jx_th4*Jy_th3);
- float dth3_des = (1.0f/dd2) * ( Jy_th4*vDesFoot[0] - Jx_th4*vDesFoot[1] );
- float dth4_des = (1.0f/dd2) * ( -Jy_th3*vDesFoot[0] + Jx_th3*vDesFoot[1] );
-
- // Calculate error variables
- float e_x = 0;
- float e_y = 0;
- float de_x = 0;
- float de_y = 0;
+ float xDes2 = b*cos(Omega*teff + phase) + x0;
+ float yDes2 = a*sin(Omega*teff + phase) + y0;
+ float vxDes2 = 0;//-b*Omega*sin(Omega*teff + phase);
+ float vyDes2 = 0; //a*Omega*cos(Omega*teff + phase);
- float e_x2 = 0;
- float e_y2 = 0;
- float de_x2 = 0;
- float de_y2 = 0;
+ float e_x2 = xDes2 - xFoot2;
+ float e_y2 = yDes2 - yFoot2;
+ float de_x2 = vxDes2 - dxFoot2;
+ float de_y2 = vyDes2 - dyFoot2;
+
+
// Calculate virtual force on foot
- float fx = K_xx*(rDesFoot[0]-xFoot) +K_xy*(rDesFoot[1]-yFoot)+D_xx*(vDesFoot[0]-dxFoot)+D_xy*(vDesFoot[1]-dyFoot);
- float fy = K_xy*(rDesFoot[0]-xFoot) + K_yy*(rDesFoot[1]-yFoot) + D_xy*(vDesFoot[0]-xFoot)+D_yy*(vDesFoot[1]-dyFoot);
+ 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 fx2 = K_xx2*(rDesFoot2[0]-xFoot2) +K_xy*(rDesFoot2[1]-yFoot2)+D_xx2*(vDesFoot2[0]-dxFoot2)+D_xy2*(vDesFoot2[1]-dyFoot2);
- float fy2 = K_xy2*(rDesFoot2[0]-xFoot2) + K_yy*(rDesFoot2[1]-yFoot2) + D_xy2*(vDesFoot2[0]-xFoot2)+D_yy2*(vDesFoot2[1]-dyFoot2);
+ float fx2 = K_xx2*(e_x2) +K_xy2*(e_y2)+D_xx2*(de_x2)+D_xy2*(de_y2);
+ float fy2 = K_xy2*(e_x2) + K_yy2*(e_y2) + D_xy2*(de_x2)+D_yy2*(de_y2);
// Set desired currents
current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;
current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;
- current_des3 = (Jx_th1*fx+Jy_th1*fy)/k_t;
- current_des4 = (Jx_th2*fx+Jy_th2*fy)/k_t;
+ current_des3 = (Jx_th1*fx2+Jy_th1*fy2)/k_t;
+ current_des4 = (Jx_th2*fx2+Jy_th2*fy2)/k_t;
@@ -500,26 +528,32 @@
output_data[9] = current_des2;
output_data[10]= duty_cycle2;
// motor 3 state
- output_data[11] = angle2;
- output_data[12] = velocity2;
- output_data[13] = current2;
- output_data[14] = current_des2;
- output_data[15]= duty_cycle2;
+ output_data[11] = angle3;
+ output_data[12] = velocity3;
+ output_data[13] = current3;
+ output_data[14] = current_des3;
+ output_data[15]= duty_cycle3;
// motor 4 state
- output_data[16] = angle2;
- output_data[17] = velocity2;
- output_data[18] = current2;
- output_data[19] = current_des2;
- output_data[20]= duty_cycle2;
+ output_data[16] = angle4;
+ output_data[17] = velocity4;
+ output_data[18] = current4;
+ output_data[19] = current_des4;
+ output_data[20]= duty_cycle4;
// foot state
output_data[21] = xFoot;
output_data[22] = yFoot;
output_data[23] = dxFoot;
output_data[24] = dyFoot;
- output_data[25] = rDesFoot[0];
- output_data[26] = rDesFoot[1];
- output_data[27] = vDesFoot[0];
- output_data[28] = vDesFoot[1];
+ output_data[25] = xDes;
+ output_data[26] = yDes;
+ output_data[27] = vxDes;
+ output_data[28] = vyDes;
+
+ output_data[29] = xFoot2;
+ output_data[30] = yFoot2;
+ output_data[31] = xDes2;
+ output_data[32] = yDes2;
+
// Send data to MATLAB
server.sendData(output_data,NUM_OUTPUTS);
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed OS |
| Created: | 30 Nov 2021 |
| Imports: | 1 |
| Forks: | 0 |
| Commits: | 8 |
| Dependents: | 0 |
| Dependencies: | 5 |
| Followers: | 2 |
