2.74 test / Mbed OS hopper

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:   MatrixMath Matrix ExperimentServer QEI_pmw MotorShield

main.cpp@30:1dd3ef6acde6, 2020-11-18 (annotated)

Committer:
sehwan
Date:
Wed Nov 18 20:44:40 2020 +0000
Revision:
30:1dd3ef6acde6
Parent:
29:8b4fd3d36882 
adfs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pwensing 0:43448bf056e8 1 #include "mbed.h"
pwensing 0:43448bf056e8 2 #include "rtos.h"
pwensing 0:43448bf056e8 3 #include "EthernetInterface.h"
pwensing 0:43448bf056e8 4 #include "ExperimentServer.h"
pwensing 0:43448bf056e8 5 #include "QEI.h"
saloutos 16:f9ea2b2d410f 6 #include "BezierCurve.h"
elijahsj 6:1faceb53dabe 7 #include "MotorShield.h"
elijahsj 13:3a1f4e09789b 8 #include "HardwareSetup.h"
saloutos 26:5822d4d8dca7 9 #include "Matrix.h"
saloutos 26:5822d4d8dca7 10 #include "MatrixMath.h"
pwensing 0:43448bf056e8 11
sehwan 30:1dd3ef6acde6 12 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
pwensing 0:43448bf056e8 13
sehwan 30:1dd3ef6acde6 14 #define size_torqueTraj 20
sehwan 30:1dd3ef6acde6 15 #define size_jointTraj 20
sehwan 30:1dd3ef6acde6 16 #define size_inputs 14+9*size_torqueTraj
sehwan 30:1dd3ef6acde6 17 #define size_outputs 30
saloutos 16:f9ea2b2d410f 18
saloutos 16:f9ea2b2d410f 19 // Initializations
pwensing 0:43448bf056e8 20 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 21 ExperimentServer server; // Object that lets us communicate with MATLAB
elijahsj 5:1ab9b2527794 22 Timer t; // Timer to measure elapsed time of experiment
elijahsj 5:1ab9b2527794 23
sehwan 30:1dd3ef6acde6 24 // Leg motors:
elijahsj 5:1ab9b2527794 25 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 26 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
sehwan 30:1dd3ef6acde6 27
sehwan 30:1dd3ef6acde6 28 // Arm motor:
elijahsj 5:1ab9b2527794 29 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
sehwan 30:1dd3ef6acde6 30
sehwan 30:1dd3ef6acde6 31 // Extra
elijahsj 5:1ab9b2527794 32 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 33
elijahsj 12:84a6dcb60422 34 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
saloutos 16:f9ea2b2d410f 35 Ticker currentLoop;
saloutos 16:f9ea2b2d410f 36
sehwan 30:1dd3ef6acde6 37 // Variables for q1 (hip)
saloutos 16:f9ea2b2d410f 38 float current1;
saloutos 16:f9ea2b2d410f 39 float current_des1 = 0;
saloutos 16:f9ea2b2d410f 40 float prev_current_des1 = 0;
saloutos 16:f9ea2b2d410f 41 float current_int1 = 0;
saloutos 16:f9ea2b2d410f 42 float angle1;
saloutos 16:f9ea2b2d410f 43 float velocity1;
saloutos 16:f9ea2b2d410f 44 float duty_cycle1;
saloutos 16:f9ea2b2d410f 45 float angle1_init;
saloutos 16:f9ea2b2d410f 46
sehwan 30:1dd3ef6acde6 47 // Variables for q2 (knee)
saloutos 16:f9ea2b2d410f 48 float current2;
saloutos 16:f9ea2b2d410f 49 float current_des2 = 0;
saloutos 16:f9ea2b2d410f 50 float prev_current_des2 = 0;
saloutos 16:f9ea2b2d410f 51 float current_int2 = 0;
saloutos 16:f9ea2b2d410f 52 float angle2;
saloutos 16:f9ea2b2d410f 53 float velocity2;
saloutos 16:f9ea2b2d410f 54 float duty_cycle2;
saloutos 16:f9ea2b2d410f 55 float angle2_init;
saloutos 16:f9ea2b2d410f 56
sehwan 30:1dd3ef6acde6 57 // Variables for q3 (arm)
sehwan 30:1dd3ef6acde6 58 float current3;
sehwan 30:1dd3ef6acde6 59 float current_des3 = 0;
sehwan 30:1dd3ef6acde6 60 float prev_current_des3 = 0;
sehwan 30:1dd3ef6acde6 61 float current_int3 = 0;
sehwan 30:1dd3ef6acde6 62 float angle3;
sehwan 30:1dd3ef6acde6 63 float velocity3;
sehwan 30:1dd3ef6acde6 64 float duty_cycle3;
sehwan 30:1dd3ef6acde6 65 float angle3_init;
sehwan 30:1dd3ef6acde6 66
saloutos 16:f9ea2b2d410f 67 // Fixed kinematic parameters
saloutos 16:f9ea2b2d410f 68 const float l_OA=.011;
saloutos 16:f9ea2b2d410f 69 const float l_OB=.042;
saloutos 16:f9ea2b2d410f 70 const float l_AC=.096;
saloutos 16:f9ea2b2d410f 71 const float l_DE=.091;
saloutos 26:5822d4d8dca7 72 const float m1 =.0393 + .2;
saloutos 26:5822d4d8dca7 73 const float m2 =.0368;
saloutos 26:5822d4d8dca7 74 const float m3 = .00783;
saloutos 26:5822d4d8dca7 75 const float m4 = .0155;
saloutos 26:5822d4d8dca7 76 const float I1 = 0.0000251; //25.1 * 10^-6;
saloutos 26:5822d4d8dca7 77 const float I2 = 0.0000535; //53.5 * 10^-6;
saloutos 26:5822d4d8dca7 78 const float I3 = 0.00000925; //9.25 * 10^-6;
saloutos 26:5822d4d8dca7 79 const float I4 = 0.0000222; //22.176 * 10^-6;
saloutos 26:5822d4d8dca7 80 const float l_O_m1=0.032;
saloutos 26:5822d4d8dca7 81 const float l_B_m2=0.0344;
saloutos 26:5822d4d8dca7 82 const float l_A_m3=0.0622;
saloutos 26:5822d4d8dca7 83 const float l_C_m4=0.0610;
saloutos 27:5d60c6ab6d0a 84 const float N = 18.75;
saloutos 27:5d60c6ab6d0a 85 const float Ir = 0.0035/pow(N,2);
saloutos 16:f9ea2b2d410f 86
sehwan 30:1dd3ef6acde6 87 // Design parameters
sehwan 30:1dd3ef6acde6 88 const float m_body = 0.186+0.211;
sehwan 30:1dd3ef6acde6 89 const float l_body = 0.04;
sehwan 30:1dd3ef6acde6 90 const float l_arm = 0.10;
sehwan 30:1dd3ef6acde6 91 const float l_cm_arm = 0.8*l_arm;
sehwan 30:1dd3ef6acde6 92 const float m_arm = 0.1;
sehwan 30:1dd3ef6acde6 93 const float I_arm = 0.00064; // calculated with I = ml^2, not from CAD
sehwan 30:1dd3ef6acde6 94
saloutos 16:f9ea2b2d410f 95 // Timing parameters
saloutos 16:f9ea2b2d410f 96 float current_control_period_us = 200.0f; // 5kHz current control loop
sehwan 30:1dd3ef6acde6 97 float impedance_control_period_us = 10000.0f; // 100 Hz impedance control loop
saloutos 16:f9ea2b2d410f 98 float start_period, traj_period, end_period;
saloutos 16:f9ea2b2d410f 99
saloutos 16:f9ea2b2d410f 100 // Control parameters
saloutos 19:562c08086d71 101 float current_Kp = 4.0f;
saloutos 19:562c08086d71 102 float current_Ki = 0.4f;
sehwan 30:1dd3ef6acde6 103 float current_int_max = 3.0f;
sehwan 30:1dd3ef6acde6 104 float K_q1;
sehwan 30:1dd3ef6acde6 105 float K_q2;
sehwan 30:1dd3ef6acde6 106 float K_q3;
sehwan 30:1dd3ef6acde6 107 float D_qd1;
sehwan 30:1dd3ef6acde6 108 float D_qd2;
sehwan 30:1dd3ef6acde6 109 float D_qd3;
sehwan 30:1dd3ef6acde6 110 int control_method = 0; // defaults to using just ff torque
saloutos 16:f9ea2b2d410f 111 float duty_max;
saloutos 16:f9ea2b2d410f 112
saloutos 16:f9ea2b2d410f 113 // Model parameters
saloutos 17:1bb5aa45826e 114 float supply_voltage = 12; // motor supply voltage
saloutos 18:21c8d94eddee 115 float R = 2.0f; // motor resistance
saloutos 18:21c8d94eddee 116 float k_t = 0.18f; // motor torque constant
saloutos 17:1bb5aa45826e 117 float nu = 0.0005; // motor viscous friction
saloutos 16:f9ea2b2d410f 118
saloutos 16:f9ea2b2d410f 119 // Current control interrupt function
saloutos 16:f9ea2b2d410f 120 void CurrentLoop()
saloutos 16:f9ea2b2d410f 121 {
saloutos 19:562c08086d71 122 // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
saloutos 16:f9ea2b2d410f 123 //use the motor shield as follows:
saloutos 16:f9ea2b2d410f 124 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
sehwan 30:1dd3ef6acde6 125
sehwan 30:1dd3ef6acde6 126 //***************************************************************************
sehwan 30:1dd3ef6acde6 127 //HIP MOTOR
saloutos 18:21c8d94eddee 128 current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
saloutos 18:21c8d94eddee 129 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
saloutos 18:21c8d94eddee 130 float err_c1 = current_des1 - current1; // current errror
saloutos 18:21c8d94eddee 131 current_int1 += err_c1; // integrate error
saloutos 18:21c8d94eddee 132 current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max); // anti-windup
saloutos 18:21c8d94eddee 133 float ff1 = R*current_des1 + k_t*velocity1; // feedforward terms
saloutos 18:21c8d94eddee 134 duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage; // PI current controller
saloutos 16:f9ea2b2d410f 135
saloutos 16:f9ea2b2d410f 136 float absDuty1 = abs(duty_cycle1);
saloutos 16:f9ea2b2d410f 137 if (absDuty1 > duty_max) {
saloutos 16:f9ea2b2d410f 138 duty_cycle1 *= duty_max / absDuty1;
saloutos 16:f9ea2b2d410f 139 absDuty1 = duty_max;
saloutos 16:f9ea2b2d410f 140 }
saloutos 16:f9ea2b2d410f 141 if (duty_cycle1 < 0) { // backwards
saloutos 16:f9ea2b2d410f 142 motorShield.motorAWrite(absDuty1, 1);
saloutos 16:f9ea2b2d410f 143 } else { // forwards
saloutos 16:f9ea2b2d410f 144 motorShield.motorAWrite(absDuty1, 0);
saloutos 16:f9ea2b2d410f 145 }
saloutos 16:f9ea2b2d410f 146 prev_current_des1 = current_des1;
saloutos 16:f9ea2b2d410f 147
sehwan 30:1dd3ef6acde6 148 //****************************************************************************
sehwan 30:1dd3ef6acde6 149 //KNEE MOTOR
saloutos 18:21c8d94eddee 150 current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
saloutos 18:21c8d94eddee 151 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
saloutos 18:21c8d94eddee 152 float err_c2 = current_des2 - current2; // current error
saloutos 18:21c8d94eddee 153 current_int2 += err_c2; // integrate error
saloutos 18:21c8d94eddee 154 current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max); // anti-windup
saloutos 18:21c8d94eddee 155 float ff2 = R*current_des2 + k_t*velocity2; // feedforward terms
saloutos 18:21c8d94eddee 156 duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage; // PI current controller
saloutos 16:f9ea2b2d410f 157
saloutos 16:f9ea2b2d410f 158 float absDuty2 = abs(duty_cycle2);
saloutos 16:f9ea2b2d410f 159 if (absDuty2 > duty_max) {
saloutos 16:f9ea2b2d410f 160 duty_cycle2 *= duty_max / absDuty2;
saloutos 16:f9ea2b2d410f 161 absDuty2 = duty_max;
saloutos 16:f9ea2b2d410f 162 }
saloutos 16:f9ea2b2d410f 163 if (duty_cycle2 < 0) { // backwards
saloutos 16:f9ea2b2d410f 164 motorShield.motorBWrite(absDuty2, 1);
saloutos 16:f9ea2b2d410f 165 } else { // forwards
saloutos 16:f9ea2b2d410f 166 motorShield.motorBWrite(absDuty2, 0);
saloutos 16:f9ea2b2d410f 167 }
saloutos 16:f9ea2b2d410f 168 prev_current_des2 = current_des2;
saloutos 16:f9ea2b2d410f 169
sehwan 30:1dd3ef6acde6 170 //***************************************************************************
sehwan 30:1dd3ef6acde6 171 //ARM MOTOR
sehwan 30:1dd3ef6acde6 172 current3 = -(((float(motorShield.readCurrentC())/65536.0f)*30.0f)-15.0f); // measure current
sehwan 30:1dd3ef6acde6 173 velocity3 = encoderC.getVelocity() * PULSE_TO_RAD; // measure velocity
sehwan 30:1dd3ef6acde6 174 float err_c3 = current_des3 - current3; // current error
sehwan 30:1dd3ef6acde6 175 current_int3 += err_c3; // integrate error
sehwan 30:1dd3ef6acde6 176 current_int3 = fmaxf( fminf(current_int3, current_int_max), -current_int_max); // anti-windup
sehwan 30:1dd3ef6acde6 177 float ff3 = R*current_des3 + k_t*velocity3; // feedforward terms
sehwan 30:1dd3ef6acde6 178 duty_cycle3 = (ff3 + current_Kp*err_c3 + current_Ki*current_int3)/supply_voltage; // PI current controller
sehwan 30:1dd3ef6acde6 179
sehwan 30:1dd3ef6acde6 180 float absDuty3 = abs(duty_cycle3);
sehwan 30:1dd3ef6acde6 181 if (absDuty3 > duty_max) {
sehwan 30:1dd3ef6acde6 182 duty_cycle3 *= duty_max / absDuty3;
sehwan 30:1dd3ef6acde6 183 absDuty3 = duty_max;
sehwan 30:1dd3ef6acde6 184 }
sehwan 30:1dd3ef6acde6 185 if (duty_cycle3 < 0) { // backwards
sehwan 30:1dd3ef6acde6 186 motorShield.motorCWrite(absDuty3, 1);
sehwan 30:1dd3ef6acde6 187 } else { // forwards
sehwan 30:1dd3ef6acde6 188 motorShield.motorCWrite(absDuty3, 0);
sehwan 30:1dd3ef6acde6 189 }
sehwan 30:1dd3ef6acde6 190 prev_current_des3 = current_des3;
saloutos 16:f9ea2b2d410f 191 }
elijahsj 6:1faceb53dabe 192
elijahsj 4:7a1b35f081bb 193 int main (void)
elijahsj 4:7a1b35f081bb 194 {
sehwan 30:1dd3ef6acde6 195 // Object for torque profile
sehwan 30:1dd3ef6acde6 196 //BezierCurve tauDes_bez(3,size_torqueTraj-1);
saloutos 17:1bb5aa45826e 197
sehwan 30:1dd3ef6acde6 198 // Object for joint position profile
sehwan 30:1dd3ef6acde6 199 //BezierCurve qDes_bez(3,size_jointTraj-1);
sehwan 30:1dd3ef6acde6 200
sehwan 30:1dd3ef6acde6 201 // Object for joint velocity profile
sehwan 30:1dd3ef6acde6 202 //BezierCurve qdDes_bez(3,size_jointTraj-1);
saloutos 17:1bb5aa45826e 203
pwensing 0:43448bf056e8 204 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 205 server.attachTerminal(pc);
pwensing 0:43448bf056e8 206 server.init();
elijahsj 13:3a1f4e09789b 207
pwensing 0:43448bf056e8 208 // Continually get input from MATLAB and run experiments
sehwan 30:1dd3ef6acde6 209 float input_params[size_inputs];
elijahsj 5:1ab9b2527794 210 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 211
pwensing 0:43448bf056e8 212 while(1) {
saloutos 16:f9ea2b2d410f 213 // If there are new inputs, this code will run
sehwan 30:1dd3ef6acde6 214 if (server.getParams(input_params,size_inputs)) {
sehwan 30:1dd3ef6acde6 215
saloutos 17:1bb5aa45826e 216 // Get inputs from MATLAB
saloutos 16:f9ea2b2d410f 217 start_period = input_params[0]; // First buffer time, before trajectory
sehwan 30:1dd3ef6acde6 218 end_period = input_params[1]; // Second buffer time, after trajectory
sehwan 30:1dd3ef6acde6 219 traj_period = input_params[2]; // Total trajectory time
sehwan 30:1dd3ef6acde6 220
sehwan 30:1dd3ef6acde6 221 angle1_init = input_params[3]; // Initial angle for q1 (hip, rad)
sehwan 30:1dd3ef6acde6 222 angle2_init = input_params[4]; // Initial angle for q2 (knee, rad)
sehwan 30:1dd3ef6acde6 223 angle3_init = input_params[5]; // Initial angle for q3 (arm, rad)
sehwan 30:1dd3ef6acde6 224
sehwan 30:1dd3ef6acde6 225 K_q1 = input_params[6]; // Joint space stiffness for hip (N/rad)
sehwan 30:1dd3ef6acde6 226 K_q2 = input_params[7]; // Joint space stiffness for knee (N/rad)
sehwan 30:1dd3ef6acde6 227 K_q3 = input_params[8]; // Joint space stiffness for arm (N/rad)
sehwan 30:1dd3ef6acde6 228 D_qd1 = input_params[9]; // Joint space damping for arm (Ns/rad)
sehwan 30:1dd3ef6acde6 229 D_qd2 = input_params[10]; // Joint space damping for knee (Ns/rad)
sehwan 30:1dd3ef6acde6 230 D_qd3 = input_params[11]; // Joint space damping for hip (Ns/rad)
sehwan 30:1dd3ef6acde6 231
sehwan 30:1dd3ef6acde6 232 control_method = int(input_params[12]); // Controller choices: feedfwd torque = 0, PD control = 1, both = 2
sehwan 30:1dd3ef6acde6 233
sehwan 30:1dd3ef6acde6 234 duty_max = input_params[13]; // Maximum duty factor
sehwan 30:1dd3ef6acde6 235
sehwan 30:1dd3ef6acde6 236 //**************************************************************
sehwan 30:1dd3ef6acde6 237 // LOADING OPTIMIZED PROFILES AND GENERATING BEZIER TRAJECTORIES
sehwan 30:1dd3ef6acde6 238
sehwan 30:1dd3ef6acde6 239 int last_index = 14; // index to track where profiles begin and end
sehwan 30:1dd3ef6acde6 240
sehwan 30:1dd3ef6acde6 241 // Load torque profile:
sehwan 30:1dd3ef6acde6 242 float torque_profile[3*(size_torqueTraj)];
sehwan 30:1dd3ef6acde6 243 for(int i = 0; i < 3*(size_torqueTraj); i++) {
sehwan 30:1dd3ef6acde6 244 torque_profile[i] = input_params[last_index+i];
saloutos 16:f9ea2b2d410f 245 }
sehwan 30:1dd3ef6acde6 246 //tauDes_bez.setPoints(torque_profile);
sehwan 30:1dd3ef6acde6 247 last_index = last_index + 3*(size_torqueTraj);
sehwan 30:1dd3ef6acde6 248
sehwan 30:1dd3ef6acde6 249
sehwan 30:1dd3ef6acde6 250 // Load joint angle profile:
sehwan 30:1dd3ef6acde6 251 float q_profile[3*(size_jointTraj)];
sehwan 30:1dd3ef6acde6 252 for(int i = 0; i < 3*(size_jointTraj); i++) {
sehwan 30:1dd3ef6acde6 253 q_profile[i] = input_params[last_index+i];
sehwan 30:1dd3ef6acde6 254 }
sehwan 30:1dd3ef6acde6 255 // qDes_bez.setPoints(q_profile);
sehwan 30:1dd3ef6acde6 256 last_index = last_index + 3*(size_jointTraj);
sehwan 30:1dd3ef6acde6 257
sehwan 30:1dd3ef6acde6 258
sehwan 30:1dd3ef6acde6 259 // Load joint velocity profile:
sehwan 30:1dd3ef6acde6 260 float qd_profile[3*(size_jointTraj)];
sehwan 30:1dd3ef6acde6 261 for(int i = 0; i < 3*(size_jointTraj); i++) {
sehwan 30:1dd3ef6acde6 262 qd_profile[i] = input_params[last_index+i];
sehwan 30:1dd3ef6acde6 263 }
sehwan 30:1dd3ef6acde6 264 //qdDes_bez.setPoints(qd_profile);
sehwan 30:1dd3ef6acde6 265
saloutos 16:f9ea2b2d410f 266
saloutos 16:f9ea2b2d410f 267 // Attach current loop interrupt
saloutos 16:f9ea2b2d410f 268 currentLoop.attach_us(CurrentLoop,current_control_period_us);
saloutos 16:f9ea2b2d410f 269
pwensing 0:43448bf056e8 270 // Setup experiment
pwensing 0:43448bf056e8 271 t.reset();
pwensing 0:43448bf056e8 272 t.start();
elijahsj 5:1ab9b2527794 273 encoderA.reset();
elijahsj 5:1ab9b2527794 274 encoderB.reset();
elijahsj 5:1ab9b2527794 275 encoderC.reset();
elijahsj 5:1ab9b2527794 276 encoderD.reset();
elijahsj 10:a40d180c305c 277
elijahsj 15:495333b2ccf1 278 motorShield.motorAWrite(0, 0); //turn motor A off
saloutos 16:f9ea2b2d410f 279 motorShield.motorBWrite(0, 0); //turn motor B off
sehwan 30:1dd3ef6acde6 280 motorShield.motorCWrite(0, 0); //turn motor C off
sehwan 30:1dd3ef6acde6 281
pwensing 0:43448bf056e8 282 // Run experiment
sehwan 30:1dd3ef6acde6 283
sehwan 30:1dd3ef6acde6 284 int iter = 0;
sehwan 30:1dd3ef6acde6 285
saloutos 16:f9ea2b2d410f 286 while( t.read() < start_period + traj_period + end_period) {
saloutos 16:f9ea2b2d410f 287
saloutos 19:562c08086d71 288 // Read encoders to get motor states
saloutos 16:f9ea2b2d410f 289 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
saloutos 16:f9ea2b2d410f 290 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
saloutos 16:f9ea2b2d410f 291
saloutos 16:f9ea2b2d410f 292 angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;
sehwan 30:1dd3ef6acde6 293 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
sehwan 30:1dd3ef6acde6 294
sehwan 30:1dd3ef6acde6 295 angle3 = encoderC.getPulses() * PULSE_TO_RAD + angle3_init;
sehwan 30:1dd3ef6acde6 296 velocity3 = encoderC.getVelocity() * PULSE_TO_RAD;
saloutos 16:f9ea2b2d410f 297
saloutos 16:f9ea2b2d410f 298 const float th1 = angle1;
saloutos 16:f9ea2b2d410f 299 const float th2 = angle2;
sehwan 30:1dd3ef6acde6 300 const float th3 = angle3;
saloutos 16:f9ea2b2d410f 301 const float dth1= velocity1;
saloutos 16:f9ea2b2d410f 302 const float dth2= velocity2;
sehwan 30:1dd3ef6acde6 303 const float dth3 = velocity3;
saloutos 16:f9ea2b2d410f 304
saloutos 16:f9ea2b2d410f 305 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
saloutos 16:f9ea2b2d410f 306 float teff = 0;
saloutos 16:f9ea2b2d410f 307 float vMult = 0;
saloutos 16:f9ea2b2d410f 308 if( t < start_period) {
sehwan 30:1dd3ef6acde6 309 K_q1 = 0.5;
sehwan 30:1dd3ef6acde6 310 K_q2 = 0.5;
sehwan 30:1dd3ef6acde6 311 K_q3 = 0.5;
sehwan 30:1dd3ef6acde6 312 D_qd1 = 0.01;
sehwan 30:1dd3ef6acde6 313 D_qd2 = 0.01;
sehwan 30:1dd3ef6acde6 314 D_qd3 = 0.01;
saloutos 16:f9ea2b2d410f 315 }
saloutos 16:f9ea2b2d410f 316 else if (t < start_period + traj_period)
saloutos 16:f9ea2b2d410f 317 {
sehwan 30:1dd3ef6acde6 318 K_q1 = input_params[7]; // Hip stiffness N/rad
sehwan 30:1dd3ef6acde6 319 K_q2 = input_params[8]; // Knee stiffness N/rad
sehwan 30:1dd3ef6acde6 320 K_q3 = input_params[9]; // Arm stiffness N/rad
sehwan 30:1dd3ef6acde6 321 D_qd1 = input_params[9]; // Hip damping N/(rad/s)
sehwan 30:1dd3ef6acde6 322 D_qd2 = input_params[10]; // Knee damping N/(rad/s)
sehwan 30:1dd3ef6acde6 323 D_qd3 = input_params[11]; // Arm damping N/(rad/s)
saloutos 16:f9ea2b2d410f 324 teff = (t-start_period);
saloutos 16:f9ea2b2d410f 325 vMult = 1;
saloutos 16:f9ea2b2d410f 326 }
elijahsj 4:7a1b35f081bb 327 else
saloutos 16:f9ea2b2d410f 328 {
saloutos 17:1bb5aa45826e 329 teff = traj_period;
saloutos 17:1bb5aa45826e 330 vMult = 0;
saloutos 16:f9ea2b2d410f 331 }
saloutos 16:f9ea2b2d410f 332
sehwan 30:1dd3ef6acde6 333 // desired values
sehwan 30:1dd3ef6acde6 334 float tau_des[3], q_des[3], qd_des[3];
sehwan 30:1dd3ef6acde6 335 for (int i = 0; i < 3; i++){
sehwan 30:1dd3ef6acde6 336 if (t < start_period){
sehwan 30:1dd3ef6acde6 337 tau_des[i] = 0;
sehwan 30:1dd3ef6acde6 338 q_des[i] = q_profile[i];
sehwan 30:1dd3ef6acde6 339 qd_des[i] = 0;
sehwan 30:1dd3ef6acde6 340 } else if (t < start_period + traj_period){
sehwan 30:1dd3ef6acde6 341 tau_des[i] = torque_profile[3*iter+i];
sehwan 30:1dd3ef6acde6 342 q_des[i] = q_profile[3*iter+i];
sehwan 30:1dd3ef6acde6 343 qd_des[i] = qd_profile[3*iter+i];}
sehwan 30:1dd3ef6acde6 344 else{
sehwan 30:1dd3ef6acde6 345 tau_des[i] = 0;
sehwan 30:1dd3ef6acde6 346 q_des[i] = 0;
sehwan 30:1dd3ef6acde6 347 qd_des[i] = 0;
sehwan 30:1dd3ef6acde6 348 }
sehwan 30:1dd3ef6acde6 349 }
saloutos 17:1bb5aa45826e 350
sehwan 30:1dd3ef6acde6 351
sehwan 30:1dd3ef6acde6 352 //tauDes_bez.evaluate(teff/traj_period,tau_des);
sehwan 30:1dd3ef6acde6 353 //qDes_bez.evaluate(teff/traj_period,q_des);
sehwan 30:1dd3ef6acde6 354 //qdDes_bez.evaluateDerivative(teff/traj_period,qd_des); // get qdDes from derivative of Bezier of qDes
sehwan 30:1dd3ef6acde6 355 //qdDes_bez.evaluate(teff/traj_period,qd_des); // alternatively, get qdDes directly from optimized profile. Potential error?
saloutos 17:1bb5aa45826e 356
sehwan 30:1dd3ef6acde6 357 // From old code -> not sure why velocities need to be scaled wrt traj. time. Don't think it's needed.
sehwan 30:1dd3ef6acde6 358 // qd_des[0]/=traj_period;
sehwan 30:1dd3ef6acde6 359 // qd_des[1]/=traj_period;
sehwan 30:1dd3ef6acde6 360 // qd_des[2]/=traj_period;
sehwan 30:1dd3ef6acde6 361
sehwan 30:1dd3ef6acde6 362 qd_des[0]*=vMult; // ensures zero velocity when moving to starting configuration
sehwan 30:1dd3ef6acde6 363 qd_des[1]*=vMult;
sehwan 30:1dd3ef6acde6 364 qd_des[2]*=vMult;
sehwan 30:1dd3ef6acde6 365
saloutos 26:5822d4d8dca7 366
sehwan 30:1dd3ef6acde6 367 // Calculate the forward kinematics (position and velocity)
sehwan 30:1dd3ef6acde6 368 float xFoot = sin(th1)*(l_DE - l_OA + l_OB) + l_AC*sin(th1 + th2) + l_OA*sin(th1);
sehwan 30:1dd3ef6acde6 369 float yFoot = - cos(th1)*(l_DE - l_OA + l_OB) - l_AC*cos(th1 + th2) - l_OA*cos(th1);
sehwan 30:1dd3ef6acde6 370 float xArm = l_arm*sin(th3); // assuming th3 defined relative to line coincident with body pointing down
sehwan 30:1dd3ef6acde6 371 float yArm = l_body - cos(th3);
sehwan 30:1dd3ef6acde6 372 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);
sehwan 30:1dd3ef6acde6 373 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);
sehwan 30:1dd3ef6acde6 374 float dxArm = dth3*l_arm*cos(th3);
sehwan 30:1dd3ef6acde6 375 float dyArm = dth3*sin(th3);
sehwan 30:1dd3ef6acde6 376
sehwan 30:1dd3ef6acde6 377 // Calculate the desired forward kinematics
sehwan 30:1dd3ef6acde6 378 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]);
sehwan 30:1dd3ef6acde6 379 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]);
sehwan 30:1dd3ef6acde6 380 float xArmDes = l_arm*sin(q_des[2]); // assuming th3 defined relative to line coincident with body pointing down
sehwan 30:1dd3ef6acde6 381 float yArmDes = l_body - cos(q_des[2]);
sehwan 30:1dd3ef6acde6 382 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]);
sehwan 30:1dd3ef6acde6 383 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]);
sehwan 30:1dd3ef6acde6 384 float dxArmDes = qd_des[2]*l_arm*cos(q_des[2]);
sehwan 30:1dd3ef6acde6 385 float dyArmDes = qd_des[2]*sin(q_des[2]);
saloutos 26:5822d4d8dca7 386
saloutos 29:8b4fd3d36882 387
sehwan 30:1dd3ef6acde6 388 // Calculate error variables
sehwan 30:1dd3ef6acde6 389 float e_th1 = q_des[0] - th1;
sehwan 30:1dd3ef6acde6 390 float e_th2 = q_des[1] - th2;
sehwan 30:1dd3ef6acde6 391 float e_th3 = q_des[2] - th3;
sehwan 30:1dd3ef6acde6 392 float de_th1 = qd_des[0] - dth1;
sehwan 30:1dd3ef6acde6 393 float de_th2 = qd_des[1] - dth2;
sehwan 30:1dd3ef6acde6 394 float de_th3 = qd_des[2] - dth3;
sehwan 30:1dd3ef6acde6 395
sehwan 30:1dd3ef6acde6 396 // Set desired currents
sehwan 30:1dd3ef6acde6 397 float current_ff[3], current_PD[3];
sehwan 30:1dd3ef6acde6 398 for(int i = 0; i < 3; i++){ // set feedforward currents
sehwan 30:1dd3ef6acde6 399 current_ff[i] = tau_des[i]/k_t;
sehwan 30:1dd3ef6acde6 400 }
sehwan 30:1dd3ef6acde6 401
sehwan 30:1dd3ef6acde6 402 current_PD[0] = (K_q1*(q_des[0] - th1)+D_qd1*(qd_des[0]-dth1))/k_t; // set PD currents
sehwan 30:1dd3ef6acde6 403 current_PD[1] = (K_q2*(q_des[1] - th2)+D_qd2*(qd_des[1]-dth2))/k_t;
sehwan 30:1dd3ef6acde6 404 current_PD[2] = (K_q3*(q_des[2] - th3)+D_qd3*(qd_des[2]-dth3))/k_t;
saloutos 26:5822d4d8dca7 405
sehwan 30:1dd3ef6acde6 406 if( t < start_period) {
sehwan 30:1dd3ef6acde6 407 control_method = 1;}
sehwan 30:1dd3ef6acde6 408 else{
sehwan 30:1dd3ef6acde6 409 control_method = int(input_params[12]);
sehwan 30:1dd3ef6acde6 410 }
saloutos 26:5822d4d8dca7 411
saloutos 29:8b4fd3d36882 412
sehwan 30:1dd3ef6acde6 413 // **************
sehwan 30:1dd3ef6acde6 414 // CONTROL CHOICE (there may be issues with setting current_des inside a switch statement with the current loop interrupt, make sure to check)
sehwan 30:1dd3ef6acde6 415 switch(control_method){
sehwan 30:1dd3ef6acde6 416 case 0: // feedforward torque only
sehwan 30:1dd3ef6acde6 417 pc.printf("TIME: %f \n FFWD CURRENT \n",t.read());
sehwan 30:1dd3ef6acde6 418 current_des1 = current_ff[0];
sehwan 30:1dd3ef6acde6 419 current_des2 = current_ff[1];
sehwan 30:1dd3ef6acde6 420 current_des3 = current_ff[2];
sehwan 30:1dd3ef6acde6 421 break;
sehwan 30:1dd3ef6acde6 422
sehwan 30:1dd3ef6acde6 423 case 1: // Joint PD control only
sehwan 30:1dd3ef6acde6 424 pc.printf("TIME: %f \n PD CURRENT \n",t.read());
sehwan 30:1dd3ef6acde6 425 current_des1 = current_PD[0];
sehwan 30:1dd3ef6acde6 426 current_des2 = current_PD[1];
sehwan 30:1dd3ef6acde6 427 current_des3 = current_PD[2];
sehwan 30:1dd3ef6acde6 428 break;
sehwan 30:1dd3ef6acde6 429
sehwan 30:1dd3ef6acde6 430 case 2: // both combined
sehwan 30:1dd3ef6acde6 431 pc.printf("TIME: %f \n BOTH CURRENT \n",t.read());
sehwan 30:1dd3ef6acde6 432 current_des1 = current_ff[0] + current_PD[0];
sehwan 30:1dd3ef6acde6 433 current_des2 = current_ff[1] + current_PD[1];
sehwan 30:1dd3ef6acde6 434 current_des3 = current_ff[2] + current_PD[2];
sehwan 30:1dd3ef6acde6 435 break;
sehwan 30:1dd3ef6acde6 436
sehwan 30:1dd3ef6acde6 437 default:
sehwan 30:1dd3ef6acde6 438 pc.printf("Invalid control method selector.\n");
sehwan 30:1dd3ef6acde6 439 exit(-100);
sehwan 30:1dd3ef6acde6 440 break;
sehwan 30:1dd3ef6acde6 441 }
saloutos 26:5822d4d8dca7 442
saloutos 19:562c08086d71 443 // Form output to send to MATLAB
sehwan 30:1dd3ef6acde6 444 float output_data[size_outputs];
saloutos 16:f9ea2b2d410f 445 // current time
pwensing 0:43448bf056e8 446 output_data[0] = t.read();
saloutos 16:f9ea2b2d410f 447 // motor 1 state
saloutos 16:f9ea2b2d410f 448 output_data[1] = angle1;
saloutos 16:f9ea2b2d410f 449 output_data[2] = velocity1;
saloutos 16:f9ea2b2d410f 450 output_data[3] = current1;
saloutos 16:f9ea2b2d410f 451 output_data[4] = current_des1;
saloutos 16:f9ea2b2d410f 452 output_data[5] = duty_cycle1;
saloutos 16:f9ea2b2d410f 453 // motor 2 state
saloutos 16:f9ea2b2d410f 454 output_data[6] = angle2;
saloutos 16:f9ea2b2d410f 455 output_data[7] = velocity2;
saloutos 16:f9ea2b2d410f 456 output_data[8] = current2;
saloutos 16:f9ea2b2d410f 457 output_data[9] = current_des2;
saloutos 16:f9ea2b2d410f 458 output_data[10]= duty_cycle2;
sehwan 30:1dd3ef6acde6 459 // motor 3 state
sehwan 30:1dd3ef6acde6 460 output_data[11] = angle3;
sehwan 30:1dd3ef6acde6 461 output_data[12] = velocity3;
sehwan 30:1dd3ef6acde6 462 output_data[13] = current3;
sehwan 30:1dd3ef6acde6 463 output_data[14] = current_des3;
sehwan 30:1dd3ef6acde6 464 output_data[15]= duty_cycle3;
sehwan 30:1dd3ef6acde6 465 // foot and arm state
sehwan 30:1dd3ef6acde6 466 output_data[16] = xFoot;
sehwan 30:1dd3ef6acde6 467 output_data[17] = yFoot;
sehwan 30:1dd3ef6acde6 468 output_data[18] = dxFoot;
sehwan 30:1dd3ef6acde6 469 output_data[19] = dyFoot;
sehwan 30:1dd3ef6acde6 470 output_data[20] = xArm;
sehwan 30:1dd3ef6acde6 471 output_data[21] = yArm;
sehwan 30:1dd3ef6acde6 472 output_data[22] = dxArm;
sehwan 30:1dd3ef6acde6 473 output_data[23] = dyArm;
sehwan 30:1dd3ef6acde6 474
sehwan 30:1dd3ef6acde6 475 output_data[24] = q_des[0];
sehwan 30:1dd3ef6acde6 476 output_data[25] = q_des[1];
sehwan 30:1dd3ef6acde6 477 output_data[26] = q_des[2];
sehwan 30:1dd3ef6acde6 478 output_data[27] = qd_des[0];
sehwan 30:1dd3ef6acde6 479 output_data[28] = qd_des[1];
sehwan 30:1dd3ef6acde6 480 output_data[29] = qd_des[2];
sehwan 30:1dd3ef6acde6 481
sehwan 30:1dd3ef6acde6 482 // can add calculations for more outputs as needed, currently not outputting desired position of arm and foot
elijahsj 13:3a1f4e09789b 483
pwensing 0:43448bf056e8 484 // Send data to MATLAB
sehwan 30:1dd3ef6acde6 485 server.sendData(output_data,size_outputs);
saloutos 16:f9ea2b2d410f 486
saloutos 16:f9ea2b2d410f 487 wait_us(impedance_control_period_us);
sehwan 30:1dd3ef6acde6 488 if((t > start_period)&&(t<start_period+traj_period)){
sehwan 30:1dd3ef6acde6 489 iter++;}
elijahsj 4:7a1b35f081bb 490 }
saloutos 16:f9ea2b2d410f 491
pwensing 0:43448bf056e8 492 // Cleanup after experiment
pwensing 0:43448bf056e8 493 server.setExperimentComplete();
saloutos 16:f9ea2b2d410f 494 currentLoop.detach();
elijahsj 12:84a6dcb60422 495 motorShield.motorAWrite(0, 0); //turn motor A off
saloutos 16:f9ea2b2d410f 496 motorShield.motorBWrite(0, 0); //turn motor B off
sehwan 30:1dd3ef6acde6 497 motorShield.motorCWrite(0, 0); //turn motor B off
saloutos 16:f9ea2b2d410f 498
pwensing 0:43448bf056e8 499 } // end if
saloutos 16:f9ea2b2d410f 500
pwensing 0:43448bf056e8 501 } // end while
elijahsj 10:a40d180c305c 502
elijahsj 6:1faceb53dabe 503 } // end main
elijahsj 6:1faceb53dabe 504