Saba Zerefa
2.74 team project
Dependencies: ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 2:4e581e5b39e8
- Parent:
- 1:25284247a74c
- Child:
- 3:2730130aa049
--- a/main.cpp Mon Nov 22 07:41:36 2021 +0000
+++ b/main.cpp Mon Nov 22 18:42:30 2021 +0000
@@ -10,8 +10,8 @@
#include "HardwareSetup.h"
#define BEZIER_ORDER_FOOT 7
-#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
-#define NUM_OUTPUTS 19
+#define NUM_INPUTS (21 + 2*(BEZIER_ORDER_FOOT+1))
+#define NUM_OUTPUTS 29
#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
@@ -25,7 +25,7 @@
QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
-
+float directionChange=-1;
MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
Ticker currentLoop;
@@ -49,6 +49,26 @@
float duty_cycle2;
float angle2_init;
+//Variables for q3 (leg 2 q1)
+float current3;
+float current_des3 = 0;
+float prev_current_des3 = 0;
+float current_int3 = 0;
+float angle3;
+float velocity3;
+float duty_cycle3;
+float angle3_init;
+
+//variables for q4 (leg 2 q2)
+float current4;
+float current_des4 = 0;
+float prev_current_des4 = 0;
+float current_int4 = 0;
+float angle4;
+float velocity4;
+float duty_cycle4;
+float angle4_init;
+
// Fixed kinematic parameters
const float l_OA=.011;
const float l_OB=.042;
@@ -72,6 +92,20 @@
float D_xy;
float D_yy;
+
+//Second foot:
+float current_Kp2 = 4.0f;
+float current_Ki2 = 0.4f;
+float current_int_max2 = 3.0f;
+float duty_max2;
+float K_xx2;
+float K_yy2;
+float K_xy2;
+float D_xx2;
+float D_xy2;
+float D_yy2;
+
+
// Model parameters
float supply_voltage = 12; // motor supply voltage
float R = 2.0f; // motor resistance
@@ -208,11 +242,22 @@
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
+
+ angle3_init = input_params[12];
+ angle4_init = input_params[13];
+
+ K_xx2 = input_params[14];
+ K_yy2 = input_params[15];
+ K_xy2 = input_params[16];
+ D_xx2 = input_params[17];
+ D_yy2 = input_params[18];
+ D_xy2 = input_params[19];
+ duty_max2 = input_params[20]
// 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[12+i];
+ foot_pts[i] = input_params[21+i];
}
rDesFoot_bez.setPoints(foot_pts);
@@ -229,6 +274,8 @@
motorShield.motorAWrite(0, 0); //turn motor A off
motorShield.motorBWrite(0, 0); //turn motor B off
+ motorShield.motorCWrite(0, 0);
+ motorShield.motorDWrite(0, 0);
// Run experiment
while( t.read() < start_period + traj_period + end_period) {
@@ -238,37 +285,75 @@
velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;
- velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
+ velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
+
+
+ angle3 = encoderC.getPulses() *PULSE_TO_RAD + angle3_init;
+ velocity3 = encoderC.getVelocity() * PULSE_TO_RAD;
+
+ angle4 = encoderD.getPulses() * PULSE_TO_RAD + angle4_init;
+ velocity4 = encoderD.getVelocity() * PULSE_TO_RAD;
+
const float th1 = angle1;
const float th2 = angle2;
const float dth1= velocity1;
const float dth2= velocity2;
+
+ const float th3 = angle3;
+ const float th4 = angle4;
+ const float dth3= velocity3;
+ const float dth4= velocity4;
+
+
+
// Calculate the Jacobian
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 Jx_th3 = l_AC*cos(th3+th4)+l_DE*cos(th3)+l_OB*cos(th3);
+ float Jx_th4 = l_AC*cos(th3+th4);
+ float Jy_th3 = l_AC*sin(th3+th4)+l_DE*sin(th3)+l_OB*sin(th3);
+ float Jy_th4 = l_AC*sin(th3+th4);
// Calculate the forward kinematics (position and velocity)
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;
+ float dyFoot = Jy_th1*dth1+Jy_th2*dth2;
+
+
+ float xFoot2 = l_AC*sin(th3+th4)+l_DE*sin(th3)+l_OB*sin(th3);
+ float yFoot2 = -l_AC*cos(th3+th4)-l_DE*cos(th3)-l_OB*cos(th3);
+ float dxFoot2 = Jx_th3*dth3+Jx_th4*dth4;
+ float dyFoot2 = Jy_th3*dth3+Jy_th4*dth4;
+
// 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) {
+ if (K_xx > 0 || K_yy > 0 || K_xx2 > 0 || K_yy2 >0) {
K_xx = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
K_yy = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
D_xx = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
D_yy = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
K_xy = 0;
D_xy = 0;
+
+ K_xx2=1;
+ K_yy2=1;
+ D_xx2=0.1;
+ D_yy2=0.1;
+ D_xy2=0;
+ K_xy2=0;
+
+
}
teff = 0;
}
@@ -280,6 +365,15 @@
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)
+
+ K_xx2 = input_params[14];
+ K_yy2 = input_params[15];
+ K_xy2 = input_params[16];
+ D_xx2 = input_params[17];
+ D_yy2 = input_params[18];
+ D_xy2 = input_params[19];
+
+
teff = (t-start_period);
vMult = 1;
}
@@ -298,6 +392,16 @@
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,rDesFoo2);
+ 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];
@@ -306,20 +410,43 @@
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 l_OE = sqrt( (pow(xFoot_inv2,2) + pow(yFoot_inv2,2)) );
+ 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
float e_x = 0;
float e_y = 0;
float de_x = 0;
float de_y = 0;
+
+ float e_x2 = 0;
+ float e_y2 = 0;
+ float de_x2 = 0;
+ float de_y2 = 0;
// 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 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);
+
+
// Set desired currents
current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;
current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;
@@ -373,15 +500,27 @@
output_data[8] = current2;
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;
+ // 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;
// 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[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];
// Send data to MATLAB
server.sendData(output_data,NUM_OUTPUTS);