Sebastian Uribe / Mbed OS pan_flipping

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:b304a7eb6908, 2020-11-02 (annotated)

Committer:
suribe
Date:
Mon Nov 02 19:40:34 2020 +0000
Revision:
30:b304a7eb6908
Parent:
29:a88dd750fdbd
Child:
31:4424902a0fd0
test

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
suribe 30:b304a7eb6908 12 int doni = 39;
suribe 30:b304a7eb6908 13
saloutos 16:f9ea2b2d410f 14 #define BEZIER_ORDER_FOOT 7
saloutos 17:1bb5aa45826e 15 #define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
saloutos 16:f9ea2b2d410f 16 #define NUM_OUTPUTS 19
pwensing 0:43448bf056e8 17
saloutos 16:f9ea2b2d410f 18 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
saloutos 16:f9ea2b2d410f 19
saloutos 16:f9ea2b2d410f 20 // Initializations
pwensing 0:43448bf056e8 21 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 22 ExperimentServer server; // Object that lets us communicate with MATLAB
elijahsj 5:1ab9b2527794 23 Timer t; // Timer to measure elapsed time of experiment
elijahsj 5:1ab9b2527794 24
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)
elijahsj 5:1ab9b2527794 27 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 28 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 29
elijahsj 12:84a6dcb60422 30 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
saloutos 16:f9ea2b2d410f 31 Ticker currentLoop;
saloutos 16:f9ea2b2d410f 32
saloutos 26:5822d4d8dca7 33 Matrix MassMatrix(2,2);
saloutos 26:5822d4d8dca7 34 Matrix Jacobian(2,2);
saloutos 26:5822d4d8dca7 35 Matrix JacobianT(2,2);
saloutos 26:5822d4d8dca7 36 Matrix InverseMassMatrix(2,2);
saloutos 26:5822d4d8dca7 37 Matrix temp_product(2,2);
saloutos 26:5822d4d8dca7 38 Matrix Lambda(2,2);
saloutos 26:5822d4d8dca7 39
saloutos 16:f9ea2b2d410f 40 // Variables for q1
saloutos 16:f9ea2b2d410f 41 float current1;
saloutos 16:f9ea2b2d410f 42 float current_des1 = 0;
saloutos 16:f9ea2b2d410f 43 float prev_current_des1 = 0;
saloutos 16:f9ea2b2d410f 44 float current_int1 = 0;
saloutos 16:f9ea2b2d410f 45 float angle1;
saloutos 16:f9ea2b2d410f 46 float velocity1;
saloutos 16:f9ea2b2d410f 47 float duty_cycle1;
saloutos 16:f9ea2b2d410f 48 float angle1_init;
saloutos 16:f9ea2b2d410f 49
saloutos 16:f9ea2b2d410f 50 // Variables for q2
saloutos 16:f9ea2b2d410f 51 float current2;
saloutos 16:f9ea2b2d410f 52 float current_des2 = 0;
saloutos 16:f9ea2b2d410f 53 float prev_current_des2 = 0;
saloutos 16:f9ea2b2d410f 54 float current_int2 = 0;
saloutos 16:f9ea2b2d410f 55 float angle2;
saloutos 16:f9ea2b2d410f 56 float velocity2;
saloutos 16:f9ea2b2d410f 57 float duty_cycle2;
saloutos 16:f9ea2b2d410f 58 float angle2_init;
saloutos 16:f9ea2b2d410f 59
saloutos 16:f9ea2b2d410f 60 // Fixed kinematic parameters
saloutos 16:f9ea2b2d410f 61 const float l_OA=.011;
saloutos 16:f9ea2b2d410f 62 const float l_OB=.042;
saloutos 16:f9ea2b2d410f 63 const float l_AC=.096;
saloutos 16:f9ea2b2d410f 64 const float l_DE=.091;
saloutos 26:5822d4d8dca7 65 const float m1 =.0393 + .2;
saloutos 26:5822d4d8dca7 66 const float m2 =.0368;
saloutos 26:5822d4d8dca7 67 const float m3 = .00783;
saloutos 26:5822d4d8dca7 68 const float m4 = .0155;
saloutos 26:5822d4d8dca7 69 const float I1 = 0.0000251; //25.1 * 10^-6;
saloutos 26:5822d4d8dca7 70 const float I2 = 0.0000535; //53.5 * 10^-6;
saloutos 26:5822d4d8dca7 71 const float I3 = 0.00000925; //9.25 * 10^-6;
saloutos 26:5822d4d8dca7 72 const float I4 = 0.0000222; //22.176 * 10^-6;
saloutos 26:5822d4d8dca7 73 const float l_O_m1=0.032;
saloutos 26:5822d4d8dca7 74 const float l_B_m2=0.0344;
saloutos 26:5822d4d8dca7 75 const float l_A_m3=0.0622;
saloutos 26:5822d4d8dca7 76 const float l_C_m4=0.0610;
saloutos 27:5d60c6ab6d0a 77 const float N = 18.75;
saloutos 27:5d60c6ab6d0a 78 const float Ir = 0.0035/pow(N,2);
saloutos 16:f9ea2b2d410f 79
saloutos 16:f9ea2b2d410f 80 // Timing parameters
saloutos 16:f9ea2b2d410f 81 float current_control_period_us = 200.0f; // 5kHz current control loop
saloutos 16:f9ea2b2d410f 82 float impedance_control_period_us = 1000.0f; // 1kHz impedance control loop
saloutos 16:f9ea2b2d410f 83 float start_period, traj_period, end_period;
saloutos 16:f9ea2b2d410f 84
saloutos 16:f9ea2b2d410f 85 // Control parameters
saloutos 19:562c08086d71 86 float current_Kp = 4.0f;
saloutos 19:562c08086d71 87 float current_Ki = 0.4f;
saloutos 19:562c08086d71 88 float current_int_max = 3.0f;
saloutos 16:f9ea2b2d410f 89 float duty_max;
saloutos 16:f9ea2b2d410f 90 float K_xx;
saloutos 16:f9ea2b2d410f 91 float K_yy;
saloutos 16:f9ea2b2d410f 92 float K_xy;
saloutos 16:f9ea2b2d410f 93 float D_xx;
saloutos 16:f9ea2b2d410f 94 float D_xy;
saloutos 16:f9ea2b2d410f 95 float D_yy;
saloutos 16:f9ea2b2d410f 96
saloutos 16:f9ea2b2d410f 97 // Model parameters
saloutos 17:1bb5aa45826e 98 float supply_voltage = 12; // motor supply voltage
saloutos 18:21c8d94eddee 99 float R = 2.0f; // motor resistance
saloutos 18:21c8d94eddee 100 float k_t = 0.18f; // motor torque constant
saloutos 17:1bb5aa45826e 101 float nu = 0.0005; // motor viscous friction
saloutos 16:f9ea2b2d410f 102
saloutos 16:f9ea2b2d410f 103 // Current control interrupt function
saloutos 16:f9ea2b2d410f 104 void CurrentLoop()
saloutos 16:f9ea2b2d410f 105 {
saloutos 19:562c08086d71 106 // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
saloutos 16:f9ea2b2d410f 107
saloutos 16:f9ea2b2d410f 108 //use the motor shield as follows:
saloutos 16:f9ea2b2d410f 109 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
saloutos 16:f9ea2b2d410f 110
saloutos 18:21c8d94eddee 111 current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
saloutos 18:21c8d94eddee 112 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
saloutos 18:21c8d94eddee 113 float err_c1 = current_des1 - current1; // current errror
saloutos 18:21c8d94eddee 114 current_int1 += err_c1; // integrate error
saloutos 18:21c8d94eddee 115 current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max); // anti-windup
saloutos 18:21c8d94eddee 116 float ff1 = R*current_des1 + k_t*velocity1; // feedforward terms
saloutos 18:21c8d94eddee 117 duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage; // PI current controller
saloutos 16:f9ea2b2d410f 118
saloutos 16:f9ea2b2d410f 119 float absDuty1 = abs(duty_cycle1);
saloutos 16:f9ea2b2d410f 120 if (absDuty1 > duty_max) {
saloutos 16:f9ea2b2d410f 121 duty_cycle1 *= duty_max / absDuty1;
saloutos 16:f9ea2b2d410f 122 absDuty1 = duty_max;
saloutos 16:f9ea2b2d410f 123 }
saloutos 16:f9ea2b2d410f 124 if (duty_cycle1 < 0) { // backwards
saloutos 16:f9ea2b2d410f 125 motorShield.motorAWrite(absDuty1, 1);
saloutos 16:f9ea2b2d410f 126 } else { // forwards
saloutos 16:f9ea2b2d410f 127 motorShield.motorAWrite(absDuty1, 0);
saloutos 16:f9ea2b2d410f 128 }
saloutos 16:f9ea2b2d410f 129 prev_current_des1 = current_des1;
saloutos 16:f9ea2b2d410f 130
saloutos 18:21c8d94eddee 131 current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
saloutos 18:21c8d94eddee 132 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
saloutos 18:21c8d94eddee 133 float err_c2 = current_des2 - current2; // current error
saloutos 18:21c8d94eddee 134 current_int2 += err_c2; // integrate error
saloutos 18:21c8d94eddee 135 current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max); // anti-windup
saloutos 18:21c8d94eddee 136 float ff2 = R*current_des2 + k_t*velocity2; // feedforward terms
saloutos 18:21c8d94eddee 137 duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage; // PI current controller
saloutos 16:f9ea2b2d410f 138
saloutos 16:f9ea2b2d410f 139 float absDuty2 = abs(duty_cycle2);
saloutos 16:f9ea2b2d410f 140 if (absDuty2 > duty_max) {
saloutos 16:f9ea2b2d410f 141 duty_cycle2 *= duty_max / absDuty2;
saloutos 16:f9ea2b2d410f 142 absDuty2 = duty_max;
saloutos 16:f9ea2b2d410f 143 }
saloutos 16:f9ea2b2d410f 144 if (duty_cycle2 < 0) { // backwards
saloutos 16:f9ea2b2d410f 145 motorShield.motorBWrite(absDuty2, 1);
saloutos 16:f9ea2b2d410f 146 } else { // forwards
saloutos 16:f9ea2b2d410f 147 motorShield.motorBWrite(absDuty2, 0);
saloutos 16:f9ea2b2d410f 148 }
saloutos 16:f9ea2b2d410f 149 prev_current_des2 = current_des2;
saloutos 16:f9ea2b2d410f 150
saloutos 16:f9ea2b2d410f 151 }
elijahsj 6:1faceb53dabe 152
elijahsj 4:7a1b35f081bb 153 int main (void)
elijahsj 4:7a1b35f081bb 154 {
saloutos 17:1bb5aa45826e 155
saloutos 17:1bb5aa45826e 156 // Object for 7th order Cartesian foot trajectory
saloutos 17:1bb5aa45826e 157 BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
saloutos 17:1bb5aa45826e 158
pwensing 0:43448bf056e8 159 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 160 server.attachTerminal(pc);
pwensing 0:43448bf056e8 161 server.init();
elijahsj 13:3a1f4e09789b 162
pwensing 0:43448bf056e8 163 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 164 float input_params[NUM_INPUTS];
elijahsj 5:1ab9b2527794 165 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 166
pwensing 0:43448bf056e8 167 while(1) {
saloutos 16:f9ea2b2d410f 168
saloutos 16:f9ea2b2d410f 169 // If there are new inputs, this code will run
pwensing 0:43448bf056e8 170 if (server.getParams(input_params,NUM_INPUTS)) {
saloutos 16:f9ea2b2d410f 171
saloutos 16:f9ea2b2d410f 172
saloutos 17:1bb5aa45826e 173 // Get inputs from MATLAB
saloutos 16:f9ea2b2d410f 174 start_period = input_params[0]; // First buffer time, before trajectory
saloutos 16:f9ea2b2d410f 175 traj_period = input_params[1]; // Trajectory time/length
saloutos 16:f9ea2b2d410f 176 end_period = input_params[2]; // Second buffer time, after trajectory
saloutos 16:f9ea2b2d410f 177
saloutos 16:f9ea2b2d410f 178 angle1_init = input_params[3]; // Initial angle for q1 (rad)
saloutos 16:f9ea2b2d410f 179 angle2_init = input_params[4]; // Initial angle for q2 (rad)
elijahsj 4:7a1b35f081bb 180
saloutos 16:f9ea2b2d410f 181 K_xx = input_params[5]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 182 K_yy = input_params[6]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 183 K_xy = input_params[7]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 184 D_xx = input_params[8]; // Foot damping N/(m/s)
saloutos 19:562c08086d71 185 D_yy = input_params[9]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 186 D_xy = input_params[10]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 187 duty_max = input_params[11]; // Maximum duty factor
saloutos 16:f9ea2b2d410f 188
saloutos 19:562c08086d71 189 // Get foot trajectory points
saloutos 16:f9ea2b2d410f 190 float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
saloutos 16:f9ea2b2d410f 191 for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
saloutos 17:1bb5aa45826e 192 foot_pts[i] = input_params[12+i];
saloutos 16:f9ea2b2d410f 193 }
saloutos 16:f9ea2b2d410f 194 rDesFoot_bez.setPoints(foot_pts);
saloutos 16:f9ea2b2d410f 195
saloutos 16:f9ea2b2d410f 196 // Attach current loop interrupt
saloutos 16:f9ea2b2d410f 197 currentLoop.attach_us(CurrentLoop,current_control_period_us);
saloutos 16:f9ea2b2d410f 198
pwensing 0:43448bf056e8 199 // Setup experiment
pwensing 0:43448bf056e8 200 t.reset();
pwensing 0:43448bf056e8 201 t.start();
elijahsj 5:1ab9b2527794 202 encoderA.reset();
elijahsj 5:1ab9b2527794 203 encoderB.reset();
elijahsj 5:1ab9b2527794 204 encoderC.reset();
elijahsj 5:1ab9b2527794 205 encoderD.reset();
elijahsj 10:a40d180c305c 206
elijahsj 15:495333b2ccf1 207 motorShield.motorAWrite(0, 0); //turn motor A off
saloutos 16:f9ea2b2d410f 208 motorShield.motorBWrite(0, 0); //turn motor B off
saloutos 16:f9ea2b2d410f 209
pwensing 0:43448bf056e8 210 // Run experiment
saloutos 16:f9ea2b2d410f 211 while( t.read() < start_period + traj_period + end_period) {
saloutos 16:f9ea2b2d410f 212
saloutos 19:562c08086d71 213 // Read encoders to get motor states
saloutos 16:f9ea2b2d410f 214 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
saloutos 16:f9ea2b2d410f 215 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
saloutos 16:f9ea2b2d410f 216
saloutos 16:f9ea2b2d410f 217 angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;
saloutos 16:f9ea2b2d410f 218 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
saloutos 16:f9ea2b2d410f 219
saloutos 16:f9ea2b2d410f 220 const float th1 = angle1;
saloutos 16:f9ea2b2d410f 221 const float th2 = angle2;
saloutos 16:f9ea2b2d410f 222 const float dth1= velocity1;
saloutos 16:f9ea2b2d410f 223 const float dth2= velocity2;
saloutos 16:f9ea2b2d410f 224
saloutos 16:f9ea2b2d410f 225 // Calculate the Jacobian
suribe 29:a88dd750fdbd 226 float Jx_th1 = l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1);
suribe 29:a88dd750fdbd 227 float Jx_th2 = l_AC*cos(th1+th2);
suribe 29:a88dd750fdbd 228 float Jy_th1 = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
suribe 29:a88dd750fdbd 229 float Jy_th2 = l_AC*sin(th1+th2);
saloutos 19:562c08086d71 230
saloutos 17:1bb5aa45826e 231 // Calculate the forward kinematics (position and velocity)
suribe 29:a88dd750fdbd 232 float xFoot = l_DE*sin(th1)+l_OB*sin(th1)+l_AC*sin(th1+th2);
suribe 29:a88dd750fdbd 233 float yFoot = -l_DE*cos(th1)-l_OB*cos(th1)-l_AC*cos(th1+th2);
suribe 29:a88dd750fdbd 234 float dxFoot = dth1*(l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1))+dth2*l_AC*cos(th1+th2);
suribe 29:a88dd750fdbd 235 float dyFoot = dth1*(l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1))+dth2*l_AC*sin(th1+th2);
suribe 29:a88dd750fdbd 236
saloutos 16:f9ea2b2d410f 237
saloutos 16:f9ea2b2d410f 238 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
saloutos 16:f9ea2b2d410f 239 float teff = 0;
saloutos 16:f9ea2b2d410f 240 float vMult = 0;
saloutos 16:f9ea2b2d410f 241 if( t < start_period) {
saloutos 16:f9ea2b2d410f 242 if (K_xx > 0 || K_yy > 0) {
saloutos 28:22530fdc149b 243 K_xx = 100;
saloutos 28:22530fdc149b 244 K_yy = 100;
saloutos 28:22530fdc149b 245 D_xx = 5;
saloutos 28:22530fdc149b 246 D_yy = 5;
saloutos 16:f9ea2b2d410f 247 K_xy = 0;
saloutos 16:f9ea2b2d410f 248 D_xy = 0;
saloutos 16:f9ea2b2d410f 249 }
saloutos 16:f9ea2b2d410f 250 teff = 0;
saloutos 16:f9ea2b2d410f 251 }
saloutos 16:f9ea2b2d410f 252 else if (t < start_period + traj_period)
saloutos 16:f9ea2b2d410f 253 {
saloutos 19:562c08086d71 254 K_xx = input_params[5]; // Foot stiffness N/m
saloutos 19:562c08086d71 255 K_yy = input_params[6]; // Foot stiffness N/m
saloutos 19:562c08086d71 256 K_xy = input_params[7]; // Foot stiffness N/m
saloutos 19:562c08086d71 257 D_xx = input_params[8]; // Foot damping N/(m/s)
saloutos 19:562c08086d71 258 D_yy = input_params[9]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 259 D_xy = input_params[10]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 260 teff = (t-start_period);
saloutos 16:f9ea2b2d410f 261 vMult = 1;
saloutos 16:f9ea2b2d410f 262 }
elijahsj 4:7a1b35f081bb 263 else
saloutos 16:f9ea2b2d410f 264 {
saloutos 17:1bb5aa45826e 265 teff = traj_period;
saloutos 17:1bb5aa45826e 266 vMult = 0;
saloutos 16:f9ea2b2d410f 267 }
saloutos 16:f9ea2b2d410f 268
saloutos 24:26a515ebb7cf 269 // Get desired foot positions and velocities
saloutos 16:f9ea2b2d410f 270 float rDesFoot[2] , vDesFoot[2];
saloutos 16:f9ea2b2d410f 271 rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
saloutos 16:f9ea2b2d410f 272 rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
saloutos 16:f9ea2b2d410f 273 vDesFoot[0]/=traj_period;
saloutos 16:f9ea2b2d410f 274 vDesFoot[1]/=traj_period;
saloutos 16:f9ea2b2d410f 275 vDesFoot[0]*=vMult;
saloutos 16:f9ea2b2d410f 276 vDesFoot[1]*=vMult;
saloutos 17:1bb5aa45826e 277
saloutos 19:562c08086d71 278 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
suribe 29:a88dd750fdbd 279 float xFootd = -rDesFoot[0];
suribe 29:a88dd750fdbd 280 float yFootd = rDesFoot[1];
suribe 29:a88dd750fdbd 281 float l_OE = sqrt( (pow(xFootd,2) + pow(yFootd,2)) );
saloutos 17:1bb5aa45826e 282 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)) ));
saloutos 18:21c8d94eddee 283 float th2_des = -(3.14159f - alpha);
suribe 29:a88dd750fdbd 284 float th1_des = -((3.14159f/2.0f) + atan2(yFootd,xFootd) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
saloutos 17:1bb5aa45826e 285
saloutos 17:1bb5aa45826e 286 float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
saloutos 17:1bb5aa45826e 287 float dth1_des = (1.0f/dd) * ( Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
saloutos 17:1bb5aa45826e 288 float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
saloutos 17:1bb5aa45826e 289
saloutos 16:f9ea2b2d410f 290 // Calculate error variables
saloutos 19:562c08086d71 291 float e_x = 0;
saloutos 19:562c08086d71 292 float e_y = 0;
saloutos 19:562c08086d71 293 float de_x = 0;
saloutos 19:562c08086d71 294 float de_y = 0;
saloutos 17:1bb5aa45826e 295
saloutos 16:f9ea2b2d410f 296 // Calculate virtual force on foot
suribe 29:a88dd750fdbd 297 float xdelta = -xFootd - xFoot;
suribe 29:a88dd750fdbd 298 float ydelta = yFootd - yFoot;
suribe 29:a88dd750fdbd 299 float dydelta = vDesFoot[1] - dyFoot;
suribe 29:a88dd750fdbd 300 float dxdelta = vDesFoot[0] - dxFoot;
suribe 29:a88dd750fdbd 301 float fx = K_xx*xdelta+K_xy*ydelta+D_xx*dxdelta+D_xy*dydelta;
suribe 29:a88dd750fdbd 302 float fy = K_yy*ydelta+K_xy*xdelta+D_yy*dydelta+D_xy*dxdelta;
suribe 29:a88dd750fdbd 303
suribe 29:a88dd750fdbd 304 float t1 = Jx_th1*fx + Jy_th1*fy;
suribe 29:a88dd750fdbd 305 float t2 = Jx_th2*fx + Jy_th2*fy;
saloutos 26:5822d4d8dca7 306
saloutos 26:5822d4d8dca7 307 // Calculate mass matrix elements
suribe 29:a88dd750fdbd 308 // float M11 = I1+I2+I3+I4+Ir+Ir*pow(N,2)+pow(l_AC,2)*m4+pow(l_A_m3,2)*m3
suribe 29:a88dd750fdbd 309 // +pow(l_B_m2,2)*m2+pow(l_C_m4,2)*m4+pow(l_OA,2)*m3+pow(l_OB,2)*m2+pow(l_OA,2)*m4
suribe 29:a88dd750fdbd 310 // +pow(l_O_m1,2)*m1+2*l_C_m4*l_OA*m4+2*l_AC*l_C_m4*m4*cos(th2)+2*l_AC*l_OA*m4*cos(th2)
suribe 29:a88dd750fdbd 311 // +2*l_A_m3*l_OA*m3*cos(th2)+2*l_B_m2*l_OB*m2*cos(th2);
suribe 29:a88dd750fdbd 312 //
suribe 29:a88dd750fdbd 313 // float M12 = I2+I3+pow(l_AC,2)*m4+pow(l_A_m3,2)*m3+pow(l_B_m2,2)*m2+Ir*N+l_AC*l_C_m4*m4*cos(th2)
suribe 29:a88dd750fdbd 314 // +l_AC*l_OA*m4*cos(th2)+l_A_m3*l_OA*m3*cos(th2)+l_B_m2*l_OB*m2*cos(th2);
suribe 29:a88dd750fdbd 315 //
suribe 29:a88dd750fdbd 316 // float M22 = pow(Ir*N,2)+m4*pow(l_AC,2)+m3*pow(l_A_m3,2)+m2*pow(l_B_m2,2)+I2+I3;
saloutos 26:5822d4d8dca7 317
suribe 29:a88dd750fdbd 318 float M11 = I1 + I2 + I3 + I4 + Ir + Ir*N*N + l_AC*l_AC*m4 + l_A_m3*l_A_m3*m3 + l_B_m2*l_B_m2*m2 + l_C_m4*l_C_m4*m4 + l_OA*l_OA*m3 + l_OB*l_OB*m2 + l_OA*l_OA*m4 + l_O_m1*l_O_m1*m1 + 2*l_C_m4*l_OA*m4 + 2*l_AC*l_C_m4*m4*cos(th2) + 2*l_AC*l_OA*m4*cos(th2) + 2*l_A_m3*l_OA*m3*cos(th2) + 2*l_B_m2*l_OB*m2*cos(th2);
suribe 29:a88dd750fdbd 319 float M12 = I2 + I3 + l_AC*l_AC*m4 + l_A_m3*l_A_m3*m3 + l_B_m2*l_B_m2*m2 + Ir*N + l_AC*l_C_m4*m4*cos(th2) + l_AC*l_OA*m4*cos(th2) + l_A_m3*l_OA*m3*cos(th2) + l_B_m2*l_OB*m2*cos(th2);
suribe 29:a88dd750fdbd 320 float M22 = Ir*N*N + m4*l_AC*l_AC + m3*l_A_m3*l_A_m3 + m2*l_B_m2*l_B_m2 + I2 + I3;
suribe 29:a88dd750fdbd 321
saloutos 26:5822d4d8dca7 322 // Populate mass matrix
saloutos 26:5822d4d8dca7 323 MassMatrix.Clear();
saloutos 26:5822d4d8dca7 324 MassMatrix << M11 << M12
saloutos 26:5822d4d8dca7 325 << M12 << M22;
saloutos 26:5822d4d8dca7 326
saloutos 26:5822d4d8dca7 327 // Populate Jacobian matrix
saloutos 26:5822d4d8dca7 328 Jacobian.Clear();
saloutos 26:5822d4d8dca7 329 Jacobian << Jx_th1 << Jx_th2
saloutos 26:5822d4d8dca7 330 << Jy_th1 << Jy_th2;
saloutos 26:5822d4d8dca7 331
saloutos 26:5822d4d8dca7 332 // Calculate Lambda matrix
saloutos 26:5822d4d8dca7 333 JacobianT = MatrixMath::Transpose(Jacobian);
saloutos 26:5822d4d8dca7 334 InverseMassMatrix = MatrixMath::Inv(MassMatrix);
saloutos 26:5822d4d8dca7 335 temp_product = Jacobian*InverseMassMatrix*JacobianT;
saloutos 26:5822d4d8dca7 336 Lambda = MatrixMath::Inv(temp_product);
saloutos 26:5822d4d8dca7 337
saloutos 26:5822d4d8dca7 338 // Pull elements of Lambda matrix
saloutos 26:5822d4d8dca7 339 float L11 = Lambda.getNumber(1,1);
saloutos 26:5822d4d8dca7 340 float L12 = Lambda.getNumber(1,2);
saloutos 26:5822d4d8dca7 341 float L21 = Lambda.getNumber(2,1);
suribe 29:a88dd750fdbd 342 float L22 = Lambda.getNumber(2,2);
suribe 29:a88dd750fdbd 343 // float L11 = 1;
suribe 29:a88dd750fdbd 344 // float L12 = 0;
suribe 29:a88dd750fdbd 345 // float L21 = 0;
suribe 29:a88dd750fdbd 346 // float L22 = 1;
suribe 29:a88dd750fdbd 347
suribe 29:a88dd750fdbd 348 // Calculate desired motor torqu
suribe 29:a88dd750fdbd 349 float t1_op = (Jx_th1*L11+Jy_th1*L21)*fx + (Jx_th1*L12+Jy_th1*L22)*fy;
suribe 29:a88dd750fdbd 350 float t2_op = (Jx_th2*L11+Jy_th2*L21)*fx + (Jx_th2*L12+Jy_th2*L22)*fy;
saloutos 18:21c8d94eddee 351
saloutos 19:562c08086d71 352 // Set desired currents
suribe 29:a88dd750fdbd 353 current_des1 = t1_op/k_t;
suribe 29:a88dd750fdbd 354 current_des2 = t2_op/k_t;
saloutos 26:5822d4d8dca7 355
saloutos 19:562c08086d71 356 // Form output to send to MATLAB
saloutos 16:f9ea2b2d410f 357 float output_data[NUM_OUTPUTS];
saloutos 16:f9ea2b2d410f 358 // current time
pwensing 0:43448bf056e8 359 output_data[0] = t.read();
saloutos 16:f9ea2b2d410f 360 // motor 1 state
saloutos 16:f9ea2b2d410f 361 output_data[1] = angle1;
saloutos 16:f9ea2b2d410f 362 output_data[2] = velocity1;
saloutos 16:f9ea2b2d410f 363 output_data[3] = current1;
saloutos 16:f9ea2b2d410f 364 output_data[4] = current_des1;
saloutos 16:f9ea2b2d410f 365 output_data[5] = duty_cycle1;
saloutos 16:f9ea2b2d410f 366 // motor 2 state
saloutos 16:f9ea2b2d410f 367 output_data[6] = angle2;
saloutos 16:f9ea2b2d410f 368 output_data[7] = velocity2;
saloutos 16:f9ea2b2d410f 369 output_data[8] = current2;
saloutos 16:f9ea2b2d410f 370 output_data[9] = current_des2;
saloutos 16:f9ea2b2d410f 371 output_data[10]= duty_cycle2;
saloutos 16:f9ea2b2d410f 372 // foot state
saloutos 16:f9ea2b2d410f 373 output_data[11] = xFoot;
saloutos 16:f9ea2b2d410f 374 output_data[12] = yFoot;
saloutos 17:1bb5aa45826e 375 output_data[13] = dxFoot;
saloutos 17:1bb5aa45826e 376 output_data[14] = dyFoot;
saloutos 17:1bb5aa45826e 377 output_data[15] = rDesFoot[0];
saloutos 17:1bb5aa45826e 378 output_data[16] = rDesFoot[1];
saloutos 17:1bb5aa45826e 379 output_data[17] = vDesFoot[0];
saloutos 17:1bb5aa45826e 380 output_data[18] = vDesFoot[1];
elijahsj 13:3a1f4e09789b 381
pwensing 0:43448bf056e8 382 // Send data to MATLAB
pwensing 0:43448bf056e8 383 server.sendData(output_data,NUM_OUTPUTS);
saloutos 16:f9ea2b2d410f 384
saloutos 16:f9ea2b2d410f 385 wait_us(impedance_control_period_us);
elijahsj 4:7a1b35f081bb 386 }
saloutos 16:f9ea2b2d410f 387
pwensing 0:43448bf056e8 388 // Cleanup after experiment
pwensing 0:43448bf056e8 389 server.setExperimentComplete();
saloutos 16:f9ea2b2d410f 390 currentLoop.detach();
elijahsj 12:84a6dcb60422 391 motorShield.motorAWrite(0, 0); //turn motor A off
saloutos 16:f9ea2b2d410f 392 motorShield.motorBWrite(0, 0); //turn motor B off
saloutos 16:f9ea2b2d410f 393
pwensing 0:43448bf056e8 394 } // end if
saloutos 16:f9ea2b2d410f 395
pwensing 0:43448bf056e8 396 } // end while
elijahsj 10:a40d180c305c 397
elijahsj 6:1faceb53dabe 398 } // end main
elijahsj 6:1faceb53dabe 399