2.74 test
dsa
Dependencies: MatrixMath Matrix ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 30:1dd3ef6acde6
- Parent:
- 29:8b4fd3d36882
--- a/main.cpp Tue Oct 06 01:02:31 2020 +0000
+++ b/main.cpp Wed Nov 18 20:44:40 2020 +0000
@@ -9,33 +9,32 @@
#include "Matrix.h"
#include "MatrixMath.h"
-#define BEZIER_ORDER_FOOT 7
-#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
-#define NUM_OUTPUTS 19
+#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
-#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
+#define size_torqueTraj 20
+#define size_jointTraj 20
+#define size_inputs 14+9*size_torqueTraj
+#define size_outputs 30
// Initializations
Serial pc(USBTX, USBRX); // USB Serial Terminal
ExperimentServer server; // Object that lets us communicate with MATLAB
Timer t; // Timer to measure elapsed time of experiment
+// Leg motors:
QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
+
+// Arm motor:
QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
+
+// Extra
QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
Ticker currentLoop;
-Matrix MassMatrix(2,2);
-Matrix Jacobian(2,2);
-Matrix JacobianT(2,2);
-Matrix InverseMassMatrix(2,2);
-Matrix temp_product(2,2);
-Matrix Lambda(2,2);
-
-// Variables for q1
+// Variables for q1 (hip)
float current1;
float current_des1 = 0;
float prev_current_des1 = 0;
@@ -45,7 +44,7 @@
float duty_cycle1;
float angle1_init;
-// Variables for q2
+// Variables for q2 (knee)
float current2;
float current_des2 = 0;
float prev_current_des2 = 0;
@@ -55,6 +54,16 @@
float duty_cycle2;
float angle2_init;
+// Variables for q3 (arm)
+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;
+
// Fixed kinematic parameters
const float l_OA=.011;
const float l_OB=.042;
@@ -75,22 +84,31 @@
const float N = 18.75;
const float Ir = 0.0035/pow(N,2);
+// Design parameters
+const float m_body = 0.186+0.211;
+const float l_body = 0.04;
+const float l_arm = 0.10;
+const float l_cm_arm = 0.8*l_arm;
+const float m_arm = 0.1;
+const float I_arm = 0.00064; // calculated with I = ml^2, not from CAD
+
// Timing parameters
float current_control_period_us = 200.0f; // 5kHz current control loop
-float impedance_control_period_us = 1000.0f; // 1kHz impedance control loop
+float impedance_control_period_us = 10000.0f; // 100 Hz impedance control loop
float start_period, traj_period, end_period;
// Control parameters
float current_Kp = 4.0f;
float current_Ki = 0.4f;
-float current_int_max = 3.0f;
+float current_int_max = 3.0f;
+float K_q1;
+float K_q2;
+float K_q3;
+float D_qd1;
+float D_qd2;
+float D_qd3;
+int control_method = 0; // defaults to using just ff torque
float duty_max;
-float K_xx;
-float K_yy;
-float K_xy;
-float D_xx;
-float D_xy;
-float D_yy;
// Model parameters
float supply_voltage = 12; // motor supply voltage
@@ -102,10 +120,11 @@
void CurrentLoop()
{
// This loop sets the motor voltage commands using PI current controllers with feedforward terms.
-
//use the motor shield as follows:
//motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
-
+
+ //***************************************************************************
+ //HIP MOTOR
current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
velocity1 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
float err_c1 = current_des1 - current1; // current errror
@@ -126,6 +145,8 @@
}
prev_current_des1 = current_des1;
+ //****************************************************************************
+ //KNEE MOTOR
current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
float err_c2 = current_des2 - current2; // current error
@@ -146,50 +167,102 @@
}
prev_current_des2 = current_des2;
+ //***************************************************************************
+ //ARM MOTOR
+ current3 = -(((float(motorShield.readCurrentC())/65536.0f)*30.0f)-15.0f); // measure current
+ velocity3 = encoderC.getVelocity() * PULSE_TO_RAD; // measure velocity
+ float err_c3 = current_des3 - current3; // current error
+ current_int3 += err_c3; // integrate error
+ current_int3 = fmaxf( fminf(current_int3, current_int_max), -current_int_max); // anti-windup
+ float ff3 = R*current_des3 + k_t*velocity3; // feedforward terms
+ duty_cycle3 = (ff3 + current_Kp*err_c3 + current_Ki*current_int3)/supply_voltage; // PI current controller
+
+ float absDuty3 = abs(duty_cycle3);
+ if (absDuty3 > duty_max) {
+ duty_cycle3 *= duty_max / absDuty3;
+ absDuty3 = duty_max;
+ }
+ if (duty_cycle3 < 0) { // backwards
+ motorShield.motorCWrite(absDuty3, 1);
+ } else { // forwards
+ motorShield.motorCWrite(absDuty3, 0);
+ }
+ prev_current_des3 = current_des3;
}
int main (void)
{
+ // Object for torque profile
+ //BezierCurve tauDes_bez(3,size_torqueTraj-1);
- // Object for 7th order Cartesian foot trajectory
- BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
+ // Object for joint position profile
+ //BezierCurve qDes_bez(3,size_jointTraj-1);
+
+ // Object for joint velocity profile
+ //BezierCurve qdDes_bez(3,size_jointTraj-1);
// Link the terminal with our server and start it up
server.attachTerminal(pc);
server.init();
// Continually get input from MATLAB and run experiments
- float input_params[NUM_INPUTS];
+ float input_params[size_inputs];
pc.printf("%f",input_params[0]);
while(1) {
-
// If there are new inputs, this code will run
- if (server.getParams(input_params,NUM_INPUTS)) {
-
-
+ if (server.getParams(input_params,size_inputs)) {
+
// 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
-
- angle1_init = input_params[3]; // Initial angle for q1 (rad)
- angle2_init = input_params[4]; // Initial angle for q2 (rad)
-
- K_xx = input_params[5]; // Foot stiffness N/m
- 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_xy = input_params[10]; // Foot damping N/(m/s)
- duty_max = input_params[11]; // Maximum duty factor
-
- // 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];
+ end_period = input_params[1]; // Second buffer time, after trajectory
+ traj_period = input_params[2]; // Total trajectory time
+
+ angle1_init = input_params[3]; // Initial angle for q1 (hip, rad)
+ angle2_init = input_params[4]; // Initial angle for q2 (knee, rad)
+ angle3_init = input_params[5]; // Initial angle for q3 (arm, rad)
+
+ K_q1 = input_params[6]; // Joint space stiffness for hip (N/rad)
+ K_q2 = input_params[7]; // Joint space stiffness for knee (N/rad)
+ K_q3 = input_params[8]; // Joint space stiffness for arm (N/rad)
+ D_qd1 = input_params[9]; // Joint space damping for arm (Ns/rad)
+ D_qd2 = input_params[10]; // Joint space damping for knee (Ns/rad)
+ D_qd3 = input_params[11]; // Joint space damping for hip (Ns/rad)
+
+ control_method = int(input_params[12]); // Controller choices: feedfwd torque = 0, PD control = 1, both = 2
+
+ duty_max = input_params[13]; // Maximum duty factor
+
+ //**************************************************************
+ // LOADING OPTIMIZED PROFILES AND GENERATING BEZIER TRAJECTORIES
+
+ int last_index = 14; // index to track where profiles begin and end
+
+ // Load torque profile:
+ float torque_profile[3*(size_torqueTraj)];
+ for(int i = 0; i < 3*(size_torqueTraj); i++) {
+ torque_profile[i] = input_params[last_index+i];
}
- rDesFoot_bez.setPoints(foot_pts);
+ //tauDes_bez.setPoints(torque_profile);
+ last_index = last_index + 3*(size_torqueTraj);
+
+
+ // Load joint angle profile:
+ float q_profile[3*(size_jointTraj)];
+ for(int i = 0; i < 3*(size_jointTraj); i++) {
+ q_profile[i] = input_params[last_index+i];
+ }
+ // qDes_bez.setPoints(q_profile);
+ last_index = last_index + 3*(size_jointTraj);
+
+
+ // Load joint velocity profile:
+ float qd_profile[3*(size_jointTraj)];
+ for(int i = 0; i < 3*(size_jointTraj); i++) {
+ qd_profile[i] = input_params[last_index+i];
+ }
+ //qdDes_bez.setPoints(qd_profile);
+
// Attach current loop interrupt
currentLoop.attach_us(CurrentLoop,current_control_period_us);
@@ -204,8 +277,12 @@
motorShield.motorAWrite(0, 0); //turn motor A off
motorShield.motorBWrite(0, 0); //turn motor B off
-
+ motorShield.motorCWrite(0, 0); //turn motor C off
+
// Run experiment
+
+ int iter = 0;
+
while( t.read() < start_period + traj_period + end_period) {
// Read encoders to get motor states
@@ -213,47 +290,37 @@
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;
const float th1 = angle1;
const float th2 = angle2;
+ const float th3 = angle3;
const float dth1= velocity1;
const float dth2= velocity2;
-
- // Calculate the Jacobian
- float Jx_th1 = 0;
- float Jx_th2 = 0;
- float Jy_th1 = 0;
- float Jy_th2 = 0;
-
- // Calculate the forward kinematics (position and velocity)
- float xFoot = 0;
- float yFoot = 0;
- float dxFoot = 0;
- float dyFoot = 0;
+ const float dth3 = velocity3;
// 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 = 100;
- K_yy = 100;
- D_xx = 5;
- D_yy = 5;
- K_xy = 0;
- D_xy = 0;
- }
- teff = 0;
+ K_q1 = 0.5;
+ K_q2 = 0.5;
+ K_q3 = 0.5;
+ D_qd1 = 0.01;
+ D_qd2 = 0.01;
+ D_qd3 = 0.01;
}
else if (t < start_period + traj_period)
{
- K_xx = input_params[5]; // Foot stiffness N/m
- 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_xy = input_params[10]; // Foot damping N/(m/s)
+ K_q1 = input_params[7]; // Hip stiffness N/rad
+ K_q2 = input_params[8]; // Knee stiffness N/rad
+ K_q3 = input_params[9]; // Arm stiffness N/rad
+ D_qd1 = input_params[9]; // Hip damping N/(rad/s)
+ D_qd2 = input_params[10]; // Knee damping N/(rad/s)
+ D_qd3 = input_params[11]; // Arm damping N/(rad/s)
teff = (t-start_period);
vMult = 1;
}
@@ -263,78 +330,118 @@
vMult = 0;
}
- // 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;
+ // desired values
+ float tau_des[3], q_des[3], qd_des[3];
+ for (int i = 0; i < 3; i++){
+ if (t < start_period){
+ tau_des[i] = 0;
+ q_des[i] = q_profile[i];
+ qd_des[i] = 0;
+ } else if (t < start_period + traj_period){
+ tau_des[i] = torque_profile[3*iter+i];
+ q_des[i] = q_profile[3*iter+i];
+ qd_des[i] = qd_profile[3*iter+i];}
+ else{
+ tau_des[i] = 0;
+ q_des[i] = 0;
+ qd_des[i] = 0;
+ }
+ }
- // 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) )));
+
+ //tauDes_bez.evaluate(teff/traj_period,tau_des);
+ //qDes_bez.evaluate(teff/traj_period,q_des);
+ //qdDes_bez.evaluateDerivative(teff/traj_period,qd_des); // get qdDes from derivative of Bezier of qDes
+ //qdDes_bez.evaluate(teff/traj_period,qd_des); // alternatively, get qdDes directly from optimized profile. Potential error?
- 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;
-
- // Calculate virtual force on foot
- float fx = 0;
- float fy = 0;
+ // From old code -> not sure why velocities need to be scaled wrt traj. time. Don't think it's needed.
+ // qd_des[0]/=traj_period;
+ // qd_des[1]/=traj_period;
+ // qd_des[2]/=traj_period;
+
+ qd_des[0]*=vMult; // ensures zero velocity when moving to starting configuration
+ qd_des[1]*=vMult;
+ qd_des[2]*=vMult;
+
- // Calculate mass matrix elements
- float M11 = 0;
- float M12 = 0;
- float M22 = 0;
+ // Calculate the forward kinematics (position and velocity)
+ float xFoot = sin(th1)*(l_DE - l_OA + l_OB) + l_AC*sin(th1 + th2) + l_OA*sin(th1);
+ float yFoot = - cos(th1)*(l_DE - l_OA + l_OB) - l_AC*cos(th1 + th2) - l_OA*cos(th1);
+ float xArm = l_arm*sin(th3); // assuming th3 defined relative to line coincident with body pointing down
+ float yArm = l_body - cos(th3);
+ float dxFoot = dth1*(cos(th1)*(l_DE - l_OA + l_OB) + l_AC*cos(th1 + th2) + l_OA*cos(th1)) + dth2*l_AC*cos(th1 + th2);
+ float dyFoot = dth1*(sin(th1)*(l_DE - l_OA + l_OB) + l_AC*sin(th1 + th2) + l_OA*sin(th1)) + dth2*l_AC*sin(th1 + th2);
+ float dxArm = dth3*l_arm*cos(th3);
+ float dyArm = dth3*sin(th3);
+
+ // Calculate the desired forward kinematics
+ float xFootDes = sin(q_des[0])*(l_DE - l_OA + l_OB) + l_AC*sin(q_des[0] + q_des[1]) + l_OA*sin(q_des[0]);
+ float yFootDes = - cos(q_des[0])*(l_DE - l_OA + l_OB) - l_AC*cos(q_des[0] + q_des[1]) - l_OA*cos(q_des[0]);
+ float xArmDes = l_arm*sin(q_des[2]); // assuming th3 defined relative to line coincident with body pointing down
+ float yArmDes = l_body - cos(q_des[2]);
+ float dxFootDes = qd_des[0]*(cos(q_des[0])*(l_DE - l_OA + l_OB) + l_AC*cos(q_des[0] + q_des[1]) + l_OA*cos(q_des[0])) + qd_des[1]*l_AC*cos(q_des[0] + q_des[1]);
+ float dyFootDes = qd_des[0]*(sin(q_des[0])*(l_DE - l_OA + l_OB) + l_AC*sin(q_des[0] + q_des[1]) + l_OA*sin(q_des[0])) + qd_des[1]*l_AC*sin(q_des[0] + q_des[1]);
+ float dxArmDes = qd_des[2]*l_arm*cos(q_des[2]);
+ float dyArmDes = qd_des[2]*sin(q_des[2]);
-
- // Populate mass matrix
- MassMatrix.Clear();
- MassMatrix << M11 << M12
- << M12 << M22;
+ // Calculate error variables
+ float e_th1 = q_des[0] - th1;
+ float e_th2 = q_des[1] - th2;
+ float e_th3 = q_des[2] - th3;
+ float de_th1 = qd_des[0] - dth1;
+ float de_th2 = qd_des[1] - dth2;
+ float de_th3 = qd_des[2] - dth3;
+
+ // Set desired currents
+ float current_ff[3], current_PD[3];
+ for(int i = 0; i < 3; i++){ // set feedforward currents
+ current_ff[i] = tau_des[i]/k_t;
+ }
+
+ current_PD[0] = (K_q1*(q_des[0] - th1)+D_qd1*(qd_des[0]-dth1))/k_t; // set PD currents
+ current_PD[1] = (K_q2*(q_des[1] - th2)+D_qd2*(qd_des[1]-dth2))/k_t;
+ current_PD[2] = (K_q3*(q_des[2] - th3)+D_qd3*(qd_des[2]-dth3))/k_t;
- // Populate Jacobian matrix
- Jacobian.Clear();
- Jacobian << Jx_th1 << Jx_th2
- << Jy_th1 << Jy_th2;
+ if( t < start_period) {
+ control_method = 1;}
+ else{
+ control_method = int(input_params[12]);
+ }
- // Once you have copied the elements of the mass matrix, uncomment the following section
- // Calculate Lambda matrix
-// JacobianT = MatrixMath::Transpose(Jacobian);
-// InverseMassMatrix = MatrixMath::Inv(MassMatrix);
-// temp_product = Jacobian*InverseMassMatrix*JacobianT;
-// Lambda = MatrixMath::Inv(temp_product);
-
- // Pull elements of Lambda matrix
-// float L11 = Lambda.getNumber(1,1);
-// float L12 = Lambda.getNumber(1,2);
-// float L21 = Lambda.getNumber(2,1);
-// float L22 = Lambda.getNumber(2,2);
-
-
-
- // Set desired currents
- current_des1 = 0;
- current_des2 = 0;
-
-
+ // **************
+ // CONTROL CHOICE (there may be issues with setting current_des inside a switch statement with the current loop interrupt, make sure to check)
+ switch(control_method){
+ case 0: // feedforward torque only
+ pc.printf("TIME: %f \n FFWD CURRENT \n",t.read());
+ current_des1 = current_ff[0];
+ current_des2 = current_ff[1];
+ current_des3 = current_ff[2];
+ break;
+
+ case 1: // Joint PD control only
+ pc.printf("TIME: %f \n PD CURRENT \n",t.read());
+ current_des1 = current_PD[0];
+ current_des2 = current_PD[1];
+ current_des3 = current_PD[2];
+ break;
+
+ case 2: // both combined
+ pc.printf("TIME: %f \n BOTH CURRENT \n",t.read());
+ current_des1 = current_ff[0] + current_PD[0];
+ current_des2 = current_ff[1] + current_PD[1];
+ current_des3 = current_ff[2] + current_PD[2];
+ break;
+
+ default:
+ pc.printf("Invalid control method selector.\n");
+ exit(-100);
+ break;
+ }
// Form output to send to MATLAB
- float output_data[NUM_OUTPUTS];
+ float output_data[size_outputs];
// current time
output_data[0] = t.read();
// motor 1 state
@@ -349,20 +456,37 @@
output_data[8] = current2;
output_data[9] = current_des2;
output_data[10]= 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];
+ // motor 3 state
+ output_data[11] = angle3;
+ output_data[12] = velocity3;
+ output_data[13] = current3;
+ output_data[14] = current_des3;
+ output_data[15]= duty_cycle3;
+ // foot and arm state
+ output_data[16] = xFoot;
+ output_data[17] = yFoot;
+ output_data[18] = dxFoot;
+ output_data[19] = dyFoot;
+ output_data[20] = xArm;
+ output_data[21] = yArm;
+ output_data[22] = dxArm;
+ output_data[23] = dyArm;
+
+ output_data[24] = q_des[0];
+ output_data[25] = q_des[1];
+ output_data[26] = q_des[2];
+ output_data[27] = qd_des[0];
+ output_data[28] = qd_des[1];
+ output_data[29] = qd_des[2];
+
+ // can add calculations for more outputs as needed, currently not outputting desired position of arm and foot
// Send data to MATLAB
- server.sendData(output_data,NUM_OUTPUTS);
+ server.sendData(output_data,size_outputs);
wait_us(impedance_control_period_us);
+ if((t > start_period)&&(t<start_period+traj_period)){
+ iter++;}
}
// Cleanup after experiment
@@ -370,6 +494,7 @@
currentLoop.detach();
motorShield.motorAWrite(0, 0); //turn motor A off
motorShield.motorBWrite(0, 0); //turn motor B off
+ motorShield.motorCWrite(0, 0); //turn motor B off
} // end if