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@16:f9ea2b2d410f, 2020-09-24 (annotated)

Committer:
saloutos
Date:
Thu Sep 24 20:16:05 2020 +0000
Revision:
16:f9ea2b2d410f
Parent:
15:495333b2ccf1
Child:
17:1bb5aa45826e
2DOF impedance controller template code

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"
pwensing 0:43448bf056e8 9
elijahsj 13:3a1f4e09789b 10
saloutos 16:f9ea2b2d410f 11 #define BEZIER_ORDER_FOOT 7
saloutos 16:f9ea2b2d410f 12 #define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1) + 2*(BEZIER_ORDER_CURRENT+1) )
saloutos 16:f9ea2b2d410f 13 #define NUM_OUTPUTS 19
pwensing 0:43448bf056e8 14
saloutos 16:f9ea2b2d410f 15 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
saloutos 16:f9ea2b2d410f 16
saloutos 16:f9ea2b2d410f 17 // Initializations
pwensing 0:43448bf056e8 18 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 19 ExperimentServer server; // Object that lets us communicate with MATLAB
elijahsj 5:1ab9b2527794 20 Timer t; // Timer to measure elapsed time of experiment
elijahsj 5:1ab9b2527794 21
elijahsj 5:1ab9b2527794 22 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 23 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 24 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 25 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 26
elijahsj 12:84a6dcb60422 27 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
saloutos 16:f9ea2b2d410f 28 Ticker currentLoop;
saloutos 16:f9ea2b2d410f 29
saloutos 16:f9ea2b2d410f 30 // Variables for q1
saloutos 16:f9ea2b2d410f 31 float current1;
saloutos 16:f9ea2b2d410f 32 float current_des1 = 0;
saloutos 16:f9ea2b2d410f 33 float prev_current_des1 = 0;
saloutos 16:f9ea2b2d410f 34 float current_int1 = 0;
saloutos 16:f9ea2b2d410f 35 float angle1;
saloutos 16:f9ea2b2d410f 36 float angle_des1;
saloutos 16:f9ea2b2d410f 37 float velocity1;
saloutos 16:f9ea2b2d410f 38 float velocity_des1;
saloutos 16:f9ea2b2d410f 39 float duty_cycle1;
saloutos 16:f9ea2b2d410f 40 float angle1_init;
saloutos 16:f9ea2b2d410f 41
saloutos 16:f9ea2b2d410f 42 // Variables for q2
saloutos 16:f9ea2b2d410f 43 float current2;
saloutos 16:f9ea2b2d410f 44 float current_des2 = 0;
saloutos 16:f9ea2b2d410f 45 float prev_current_des2 = 0;
saloutos 16:f9ea2b2d410f 46 float current_int2 = 0;
saloutos 16:f9ea2b2d410f 47 float angle2;
saloutos 16:f9ea2b2d410f 48 float angle_des2;
saloutos 16:f9ea2b2d410f 49 float velocity2;
saloutos 16:f9ea2b2d410f 50 float velocity_des2;
saloutos 16:f9ea2b2d410f 51 float duty_cycle2;
saloutos 16:f9ea2b2d410f 52 float angle2_init;
saloutos 16:f9ea2b2d410f 53
saloutos 16:f9ea2b2d410f 54 // Fixed kinematic parameters
saloutos 16:f9ea2b2d410f 55 const float l_OA=.011;
saloutos 16:f9ea2b2d410f 56 const float l_OB=.042;
saloutos 16:f9ea2b2d410f 57 const float l_AC=.096;
saloutos 16:f9ea2b2d410f 58 const float l_DE=.091;
saloutos 16:f9ea2b2d410f 59
saloutos 16:f9ea2b2d410f 60 // Timing parameters
saloutos 16:f9ea2b2d410f 61 float current_control_period_us = 200.0f; // 5kHz current control loop
saloutos 16:f9ea2b2d410f 62 float impedance_control_period_us = 1000.0f; // 1kHz impedance control loop
saloutos 16:f9ea2b2d410f 63 float start_period, traj_period, end_period;
saloutos 16:f9ea2b2d410f 64
saloutos 16:f9ea2b2d410f 65 // Control parameters
saloutos 16:f9ea2b2d410f 66 float current_Kp = 0.7;
saloutos 16:f9ea2b2d410f 67 float current_Ki = 0.2;
saloutos 16:f9ea2b2d410f 68 float current_int_max = 1.0;
saloutos 16:f9ea2b2d410f 69 float duty_max;
saloutos 16:f9ea2b2d410f 70 float K_xx;
saloutos 16:f9ea2b2d410f 71 float K_yy;
saloutos 16:f9ea2b2d410f 72 float K_xy;
saloutos 16:f9ea2b2d410f 73 float D_xx;
saloutos 16:f9ea2b2d410f 74 float D_xy;
saloutos 16:f9ea2b2d410f 75 float D_yy;
saloutos 16:f9ea2b2d410f 76 float xFoot;
saloutos 16:f9ea2b2d410f 77 float yFoot;
saloutos 16:f9ea2b2d410f 78 float xDesFoot;
saloutos 16:f9ea2b2d410f 79 float yDesFoot;
saloutos 16:f9ea2b2d410f 80
saloutos 16:f9ea2b2d410f 81 // Model parameters
saloutos 16:f9ea2b2d410f 82 float supply_voltage = 12; // motor supply voltage
saloutos 16:f9ea2b2d410f 83 float R; // motor resistance
saloutos 16:f9ea2b2d410f 84 float k_emf; // motor torque constant
saloutos 16:f9ea2b2d410f 85 float nu1, nu2; // motor viscous friction
saloutos 16:f9ea2b2d410f 86
saloutos 16:f9ea2b2d410f 87 // Current control interrupt function
saloutos 16:f9ea2b2d410f 88 void CurrentLoop()
saloutos 16:f9ea2b2d410f 89 {
saloutos 16:f9ea2b2d410f 90 // This loop sets the motor voltage commands using PI current controllers.
saloutos 16:f9ea2b2d410f 91
saloutos 16:f9ea2b2d410f 92 //use the motor shield as follows:
saloutos 16:f9ea2b2d410f 93 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
saloutos 16:f9ea2b2d410f 94
saloutos 16:f9ea2b2d410f 95 current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
saloutos 16:f9ea2b2d410f 96 float err_c1 = current_des1 - current1; // current errror
saloutos 16:f9ea2b2d410f 97 current_int1 += err_c1; // integrate error
saloutos 16:f9ea2b2d410f 98 current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max); // anti-windup
saloutos 16:f9ea2b2d410f 99 duty_cycle1 = current_Kp*err_c1 + current_Ki*current_int1; // PI current controller
saloutos 16:f9ea2b2d410f 100
saloutos 16:f9ea2b2d410f 101 float absDuty1 = abs(duty_cycle1);
saloutos 16:f9ea2b2d410f 102 if (absDuty1 > duty_max) {
saloutos 16:f9ea2b2d410f 103 duty_cycle1 *= duty_max / absDuty1;
saloutos 16:f9ea2b2d410f 104 absDuty1 = duty_max;
saloutos 16:f9ea2b2d410f 105 }
saloutos 16:f9ea2b2d410f 106 if (duty_cycle1 < 0) { // backwards
saloutos 16:f9ea2b2d410f 107 motorShield.motorAWrite(absDuty1, 1);
saloutos 16:f9ea2b2d410f 108 } else { // forwards
saloutos 16:f9ea2b2d410f 109 motorShield.motorAWrite(absDuty1, 0);
saloutos 16:f9ea2b2d410f 110 }
saloutos 16:f9ea2b2d410f 111 prev_current_des1 = current_des1;
saloutos 16:f9ea2b2d410f 112
saloutos 16:f9ea2b2d410f 113 current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
saloutos 16:f9ea2b2d410f 114 float err_c2 = current_des2 - current2; // current error
saloutos 16:f9ea2b2d410f 115 current_int2 += err_c2; // integrate error
saloutos 16:f9ea2b2d410f 116 current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max); // anti-windup
saloutos 16:f9ea2b2d410f 117 duty_cycle2 = current_Kp*err_c2 + current_Ki*current_int2; // PI current controller
saloutos 16:f9ea2b2d410f 118
saloutos 16:f9ea2b2d410f 119 float absDuty2 = abs(duty_cycle2);
saloutos 16:f9ea2b2d410f 120 if (absDuty2 > duty_max) {
saloutos 16:f9ea2b2d410f 121 duty_cycle2 *= duty_max / absDuty2;
saloutos 16:f9ea2b2d410f 122 absDuty2 = duty_max;
saloutos 16:f9ea2b2d410f 123 }
saloutos 16:f9ea2b2d410f 124 if (duty_cycle2 < 0) { // backwards
saloutos 16:f9ea2b2d410f 125 motorShield.motorBWrite(absDuty2, 1);
saloutos 16:f9ea2b2d410f 126 } else { // forwards
saloutos 16:f9ea2b2d410f 127 motorShield.motorBWrite(absDuty2, 0);
saloutos 16:f9ea2b2d410f 128 }
saloutos 16:f9ea2b2d410f 129 prev_current_des2 = current_des2;
saloutos 16:f9ea2b2d410f 130
saloutos 16:f9ea2b2d410f 131 }
elijahsj 6:1faceb53dabe 132
elijahsj 4:7a1b35f081bb 133 int main (void)
elijahsj 4:7a1b35f081bb 134 {
pwensing 0:43448bf056e8 135 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 136 server.attachTerminal(pc);
pwensing 0:43448bf056e8 137 server.init();
elijahsj 13:3a1f4e09789b 138
pwensing 0:43448bf056e8 139 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 140 float input_params[NUM_INPUTS];
elijahsj 5:1ab9b2527794 141 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 142
pwensing 0:43448bf056e8 143 while(1) {
saloutos 16:f9ea2b2d410f 144
saloutos 16:f9ea2b2d410f 145 // If there are new inputs, this code will run
pwensing 0:43448bf056e8 146 if (server.getParams(input_params,NUM_INPUTS)) {
saloutos 16:f9ea2b2d410f 147
saloutos 16:f9ea2b2d410f 148
saloutos 16:f9ea2b2d410f 149 // Get inputs from MATLAB
saloutos 16:f9ea2b2d410f 150 start_period = input_params[0]; // First buffer time, before trajectory
saloutos 16:f9ea2b2d410f 151 traj_period = input_params[1]; // Trajectory time/length
saloutos 16:f9ea2b2d410f 152 end_period = input_params[2]; // Second buffer time, after trajectory
saloutos 16:f9ea2b2d410f 153
saloutos 16:f9ea2b2d410f 154 angle1_init = input_params[3]; // Initial angle for q1 (rad)
saloutos 16:f9ea2b2d410f 155 angle2_init = input_params[4]; // Initial angle for q2 (rad)
elijahsj 4:7a1b35f081bb 156
saloutos 16:f9ea2b2d410f 157 K_xx = input_params[5]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 158 K_yy = input_params[6]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 159 K_xy = input_params[7]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 160 D_xx = input_params[8]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 161 D_yy = input_params[9]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 162 D_xy = input_params[10]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 163 duty_max = input_params[11]; // Maximum duty factor
saloutos 16:f9ea2b2d410f 164
saloutos 16:f9ea2b2d410f 165 // TODO: check that this gets inputs correctly?
saloutos 16:f9ea2b2d410f 166 float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
saloutos 16:f9ea2b2d410f 167 for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
saloutos 16:f9ea2b2d410f 168 foot_pts[i] = input_params[21+i];
saloutos 16:f9ea2b2d410f 169 }
saloutos 16:f9ea2b2d410f 170 rDesFoot_bez.setPoints(foot_pts);
saloutos 16:f9ea2b2d410f 171
saloutos 16:f9ea2b2d410f 172 // Attach current loop interrupt
saloutos 16:f9ea2b2d410f 173 currentLoop.attach_us(CurrentLoop,current_control_period_us);
saloutos 16:f9ea2b2d410f 174
pwensing 0:43448bf056e8 175 // Setup experiment
pwensing 0:43448bf056e8 176 t.reset();
pwensing 0:43448bf056e8 177 t.start();
elijahsj 5:1ab9b2527794 178 encoderA.reset();
elijahsj 5:1ab9b2527794 179 encoderB.reset();
elijahsj 5:1ab9b2527794 180 encoderC.reset();
elijahsj 5:1ab9b2527794 181 encoderD.reset();
elijahsj 10:a40d180c305c 182
elijahsj 15:495333b2ccf1 183 motorShield.motorAWrite(0, 0); //turn motor A off
saloutos 16:f9ea2b2d410f 184 motorShield.motorBWrite(0, 0); //turn motor B off
saloutos 16:f9ea2b2d410f 185
pwensing 0:43448bf056e8 186 // Run experiment
saloutos 16:f9ea2b2d410f 187 while( t.read() < start_period + traj_period + end_period) {
saloutos 16:f9ea2b2d410f 188
saloutos 16:f9ea2b2d410f 189 // Read encoders to get motor states
saloutos 16:f9ea2b2d410f 190 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
saloutos 16:f9ea2b2d410f 191 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
saloutos 16:f9ea2b2d410f 192
saloutos 16:f9ea2b2d410f 193 angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;
saloutos 16:f9ea2b2d410f 194 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
saloutos 16:f9ea2b2d410f 195
saloutos 16:f9ea2b2d410f 196 const float th1 = angle1;
saloutos 16:f9ea2b2d410f 197 const float th2 = angle2;
saloutos 16:f9ea2b2d410f 198 const float dth1= velocity1;
saloutos 16:f9ea2b2d410f 199 const float dth2= velocity2;
saloutos 16:f9ea2b2d410f 200
saloutos 16:f9ea2b2d410f 201 // ADD YOUR CONTROL CODE HERE (CALCULATE AND SET DESIRED CURRENTS)
saloutos 16:f9ea2b2d410f 202
saloutos 16:f9ea2b2d410f 203 // Calculate the Jacobian
saloutos 16:f9ea2b2d410f 204 float Jx_th1 = 0;
saloutos 16:f9ea2b2d410f 205 float Jx_th2 = 0;
saloutos 16:f9ea2b2d410f 206 float Jy_th1 = 0;
saloutos 16:f9ea2b2d410f 207 float Jx_th2 = 0;
saloutos 16:f9ea2b2d410f 208
saloutos 16:f9ea2b2d410f 209 // Calculate the forware kinematics (position and velocity)
saloutos 16:f9ea2b2d410f 210 float xLeg = 0;
saloutos 16:f9ea2b2d410f 211 float yLeg = 0;
saloutos 16:f9ea2b2d410f 212 float dxLeg = 0;
saloutos 16:f9ea2b2d410f 213 float fyLeg = 0;
saloutos 16:f9ea2b2d410f 214
saloutos 16:f9ea2b2d410f 215 // float Jx_th1 = l_AC*cos(th1 + th2) + l_DE*cos(th1) + l_OB*cos(th1);
saloutos 16:f9ea2b2d410f 216 // float Jx_th2 = l_AC*cos(th1 + th2);
saloutos 16:f9ea2b2d410f 217 // float Jy_th1 = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
saloutos 16:f9ea2b2d410f 218 // float Jy_th2 = l_AC*sin(th1 + th2);
saloutos 16:f9ea2b2d410f 219 //
saloutos 16:f9ea2b2d410f 220 // float xLeg = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
saloutos 16:f9ea2b2d410f 221 // float yLeg = - l_AC*cos(th1 + th2) - l_DE*cos(th1) - l_OB*cos(th1);
saloutos 16:f9ea2b2d410f 222 // float dxLeg = Jx_th1 * dth1 + Jx_th2 * dth2;
saloutos 16:f9ea2b2d410f 223 // float dyLeg = Jy_th1 * dth1 + Jy_th2 * dth2;
saloutos 16:f9ea2b2d410f 224
saloutos 16:f9ea2b2d410f 225 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
saloutos 16:f9ea2b2d410f 226 float teff = 0;
saloutos 16:f9ea2b2d410f 227 float vMult = 0;
saloutos 16:f9ea2b2d410f 228 if( t < start_period) {
saloutos 16:f9ea2b2d410f 229 if (K_xx > 0 || K_yy > 0) {
saloutos 16:f9ea2b2d410f 230 K_xx = 180;
saloutos 16:f9ea2b2d410f 231 K_yy = 180;
saloutos 16:f9ea2b2d410f 232 D_xx = 2;
saloutos 16:f9ea2b2d410f 233 D_yy = 2;
saloutos 16:f9ea2b2d410f 234 K_xy = 0;
saloutos 16:f9ea2b2d410f 235 D_xy = 0;
saloutos 16:f9ea2b2d410f 236 }
saloutos 16:f9ea2b2d410f 237 teff = 0;
saloutos 16:f9ea2b2d410f 238 }
saloutos 16:f9ea2b2d410f 239 else if (t < start_period + traj_period)
saloutos 16:f9ea2b2d410f 240 {
saloutos 16:f9ea2b2d410f 241 K_xx = input_params[5]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 242 K_yy = input_params[6]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 243 K_xy = input_params[7]; // Foot stiffness N/m
saloutos 16:f9ea2b2d410f 244 D_xx = input_params[8]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 245 D_yy = input_params[9]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 246 D_xy = input_params[10]; // Foot damping N/(m/s)
saloutos 16:f9ea2b2d410f 247 teff = (t-start_period);
saloutos 16:f9ea2b2d410f 248 vMult = 1;
saloutos 16:f9ea2b2d410f 249 }
elijahsj 4:7a1b35f081bb 250 else
saloutos 16:f9ea2b2d410f 251 {
saloutos 16:f9ea2b2d410f 252 teff = traj_period;
saloutos 16:f9ea2b2d410f 253 // TODO: reset gains and vMult here?
saloutos 16:f9ea2b2d410f 254 }
saloutos 16:f9ea2b2d410f 255
saloutos 16:f9ea2b2d410f 256 float rDesFoot[2] , vDesFoot[2];
saloutos 16:f9ea2b2d410f 257 rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
saloutos 16:f9ea2b2d410f 258 rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
saloutos 16:f9ea2b2d410f 259 vDesFoot[0]/=traj_period;
saloutos 16:f9ea2b2d410f 260 vDesFoot[1]/=traj_period;
saloutos 16:f9ea2b2d410f 261 vDesFoot[0]*=vMult;
saloutos 16:f9ea2b2d410f 262 vDesFoot[1]*=vMult;
elijahsj 4:7a1b35f081bb 263
saloutos 16:f9ea2b2d410f 264 // Calculate error variables
saloutos 16:f9ea2b2d410f 265 float e_x = 0;
saloutos 16:f9ea2b2d410f 266 float e_y = 0;
saloutos 16:f9ea2b2d410f 267 float de_x = 0;
saloutos 16:f9ea2b2d410f 268 float de_y = 0;
saloutos 16:f9ea2b2d410f 269
saloutos 16:f9ea2b2d410f 270 // float e_x = ( xLeg - rDesFoot[0]);
saloutos 16:f9ea2b2d410f 271 // float e_y = ( yLeg - rDesFoot[1]);
saloutos 16:f9ea2b2d410f 272 // float de_x = ( dxLeg - vDesFoot[0]);
saloutos 16:f9ea2b2d410f 273 // float de_y = ( dyLeg - vDesFoot[1]);
saloutos 16:f9ea2b2d410f 274 //
saloutos 16:f9ea2b2d410f 275 // Calculate virtual force on foot
saloutos 16:f9ea2b2d410f 276 float fx = 0;
saloutos 16:f9ea2b2d410f 277 float fy = 0;
saloutos 16:f9ea2b2d410f 278
saloutos 16:f9ea2b2d410f 279 // float fx = -K_xx * e_x - K_xy * e_y - D_xx * de_x -D_xy * de_y;
saloutos 16:f9ea2b2d410f 280 // float fy = -K_yy * e_y - K_xy * e_x - D_yy * de_y -D_xy * de_x;
elijahsj 13:3a1f4e09789b 281
saloutos 16:f9ea2b2d410f 282 // Set desired currents
saloutos 16:f9ea2b2d410f 283 current_des1 = 0;
saloutos 16:f9ea2b2d410f 284 current_des2 = 0;
saloutos 16:f9ea2b2d410f 285
saloutos 16:f9ea2b2d410f 286 // current_des1 = (nu1*velocity1 + Jx_th1*fx + Jy_th1*fy)/k_emf;
saloutos 16:f9ea2b2d410f 287 // current_des2 = (nu2*velocity2 + Jx_th2*fx + Jy_th2*fy)/k_emf;
saloutos 16:f9ea2b2d410f 288
saloutos 16:f9ea2b2d410f 289 //Form output to send to MATLAB
saloutos 16:f9ea2b2d410f 290 float output_data[NUM_OUTPUTS];
saloutos 16:f9ea2b2d410f 291 // current time
pwensing 0:43448bf056e8 292 output_data[0] = t.read();
saloutos 16:f9ea2b2d410f 293 // motor 1 state
saloutos 16:f9ea2b2d410f 294 output_data[1] = angle1;
saloutos 16:f9ea2b2d410f 295 output_data[2] = velocity1;
saloutos 16:f9ea2b2d410f 296 output_data[3] = current1;
saloutos 16:f9ea2b2d410f 297 output_data[4] = current_des1;
saloutos 16:f9ea2b2d410f 298 output_data[5] = duty_cycle1;
saloutos 16:f9ea2b2d410f 299 // motor 2 state
saloutos 16:f9ea2b2d410f 300 output_data[6] = angle2;
saloutos 16:f9ea2b2d410f 301 output_data[7] = velocity2;
saloutos 16:f9ea2b2d410f 302 output_data[8] = current2;
saloutos 16:f9ea2b2d410f 303 output_data[9] = current_des2;
saloutos 16:f9ea2b2d410f 304 output_data[10]= duty_cycle2;
saloutos 16:f9ea2b2d410f 305 // foot state
saloutos 16:f9ea2b2d410f 306 output_data[11] = xFoot;
saloutos 16:f9ea2b2d410f 307 output_data[12] = yFoot;
saloutos 16:f9ea2b2d410f 308 output_data[13]= dxFoot;
saloutos 16:f9ea2b2d410f 309 output_data[14]= dyFoot;
saloutos 16:f9ea2b2d410f 310 output_data[15]= rDesFoot[0];
saloutos 16:f9ea2b2d410f 311 output_data[16]= rDesFoot[1];
saloutos 16:f9ea2b2d410f 312 output_data[17]= vDesFoot[0];
saloutos 16:f9ea2b2d410f 313 output_data[18]= vDesFoot[1];
elijahsj 13:3a1f4e09789b 314
pwensing 0:43448bf056e8 315 // Send data to MATLAB
pwensing 0:43448bf056e8 316 server.sendData(output_data,NUM_OUTPUTS);
saloutos 16:f9ea2b2d410f 317
saloutos 16:f9ea2b2d410f 318 wait_us(impedance_control_period_us);
elijahsj 4:7a1b35f081bb 319 }
saloutos 16:f9ea2b2d410f 320
pwensing 0:43448bf056e8 321 // Cleanup after experiment
pwensing 0:43448bf056e8 322 server.setExperimentComplete();
saloutos 16:f9ea2b2d410f 323 currentLoop.detach();
elijahsj 12:84a6dcb60422 324 motorShield.motorAWrite(0, 0); //turn motor A off
saloutos 16:f9ea2b2d410f 325 motorShield.motorBWrite(0, 0); //turn motor B off
saloutos 16:f9ea2b2d410f 326
pwensing 0:43448bf056e8 327 } // end if
saloutos 16:f9ea2b2d410f 328
pwensing 0:43448bf056e8 329 } // end while
elijahsj 10:a40d180c305c 330
elijahsj 6:1faceb53dabe 331 } // end main
elijahsj 6:1faceb53dabe 332