Julia Wyatt / Mbed OS project

Updated 6:03 pm 11/18

Dependencies:   ExperimentServer QEI_pmw MotorShield

main.cpp@25:906ee0226ebf, 2020-11-18 (annotated)

Committer:
jawyatt
Date:
Wed Nov 18 23:06:30 2020 +0000
Revision:
25:906ee0226ebf
Parent:
24:bf92a281beb8 
Updated 6:03pm 11/18

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