Sridevi Kaza
/
project
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: Bezier_Traj_Follower_Example ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 24:bf92a281beb8
- Parent:
- 23:80e05d939f8c
diff -r 80e05d939f8c -r bf92a281beb8 main.cpp
--- a/main.cpp Mon Sep 28 20:54:51 2020 +0000
+++ b/main.cpp Wed Nov 18 20:41:13 2020 +0000
@@ -104,7 +104,7 @@
prev_current_des1 = current_des1;
current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
- velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
+ velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
float err_c2 = current_des2 - current2; // current error
current_int2 += err_c2; // integrate error
current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max); // anti-windup
@@ -163,8 +163,12 @@
// Get foot trajectory points
float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
+ //float foot_pts2[2*(BEZIER_ORDER_FOOT+1)];
for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
- foot_pts[i] = input_params[12+i];
+ foot_pts[i] = input_params[12+i];
+ //pc.printf("foot_pts");
+ //pc.printf(foot_pts);
+ //foot_pts2[i] = input_params[13];
}
rDesFoot_bez.setPoints(foot_pts);
@@ -198,16 +202,16 @@
const float dth2= velocity2;
// 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);
// 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 = dth1*(l_AC*cos(th1 + th2) + l_DE*cos(th1) + l_OB*cos(th1)) + dth2*l_AC*cos(th1 + th2);
+ float dyFoot = dth1*(l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1)) + dth2*l_AC*sin(th1 + th2);
// Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
float teff = 0;
@@ -220,6 +224,7 @@
D_yy = 2; // for joint space control, set this to 0.1
K_xy = 0;
D_xy = 0;
+ //rDesFoot_bez.setPoints(foot_pts);
}
teff = 0;
}
@@ -233,6 +238,23 @@
D_xy = input_params[10]; // Foot damping N/(m/s)
teff = (t-start_period);
vMult = 1;
+ foot_pts[0] = -0.15;
+ foot_pts[1] = -0.15;
+ foot_pts[2] = -0.15;
+ foot_pts[3] = -0.15;
+ foot_pts[4] = -0.15;
+ foot_pts[5] = -0.15;
+ foot_pts[6] = -0.15;
+ foot_pts[7] = -0.15;
+ foot_pts[8] = -0.15;
+ foot_pts[9] = -0.15;
+ foot_pts[10] = -0.15;
+ foot_pts[11] = -0.15;
+ foot_pts[12] = -0.15;
+ foot_pts[13] = -0.15;
+ foot_pts[14] = -0.15;
+ foot_pts[15] = -0.15;
+ rDesFoot_bez.setPoints(foot_pts);
}
else
{
@@ -243,48 +265,55 @@
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;
+ //vDesFoot[0]/=traj_period;
+ //vDesFoot[1]/=traj_period;
+ // vDesFoot[0]*=vMult;
+ // vDesFoot[1]*=vMult;
+ 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];
+ // float xFootd = -rDesFoot[0];
+ float xFootd = foot_pts[0];
+ float yFootd = foot_pts[1];
+ //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 = 0;
- float e_y = 0;
- float de_x = 0;
- float de_y = 0;
+ float e_x = rDesFoot[0]-xFoot;
+ float e_y = rDesFoot[1]-yFoot;
+ float de_x = vDesFoot[0]-dxFoot;
+ float de_y = vDesFoot[1]-dyFoot;
// Calculate virtual force on foot
- float fx = 0;
- float fy = 0;
+ 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]-dxFoot)+D_yy*(vDesFoot[1]-dyFoot);
+
+ // torque
+ float T1 = Jx_th1*fx+Jy_th1*fy;
+ float T2 = Jx_th2*fx+Jy_th2*fy;
// Set desired currents
- current_des1 = 0;
- current_des2 = 0;
-
+ //current_des1 = 0;
+ //current_des2 = 0;
+
// 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 = 0;
-// current_des2 = 0;
+ //current_des1 = (K_xx*(th1_des-th1)+D_xx*(dth1_des-dth1))/k_t;
+ //current_des2 = (K_yy*(th2_des-th2)+D_yy*(dth2_des-dth2))/k_t;
// Cartesian impedance
// Note: As with the joint space laws, be careful with signs!
-// current_des1 = 0;
-// current_des2 = 0;
+ current_des1 = T1/k_t;
+ current_des2 = T2/k_t;
// Form output to send to MATLAB