Laura Schwendeman / Mbed OS 274TeamProject2

als;djfpoafb hnw3jg

Dependencies:   MatrixMath Matrix ExperimentServer QEI_pmw MotorShield

main.cpp@5:3d30627ae76e, 2021-11-30 (annotated)

Committer:
lschwend
Date:
Tue Nov 30 21:52:04 2021 +0000
Revision:
5:3d30627ae76e
Parent:
4:bb441c9325f4
Child:
6:7f39aa2c97da
Added ellipse and some other edits;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sabazerefa 1:25284247a74c 1 #include "mbed.h"
sabazerefa 1:25284247a74c 2 #include <stdio.h> /* printf */
sabazerefa 1:25284247a74c 3 #include <math.h> /* cos */
sabazerefa 1:25284247a74c 4 #include "rtos.h"
sabazerefa 1:25284247a74c 5 #include "EthernetInterface.h"
sabazerefa 1:25284247a74c 6 #include "ExperimentServer.h"
sabazerefa 1:25284247a74c 7 #include "QEI.h"
sabazerefa 1:25284247a74c 8 #include "BezierCurve.h"
sabazerefa 1:25284247a74c 9 #include "MotorShield.h"
sabazerefa 1:25284247a74c 10 #include "HardwareSetup.h"
sabazerefa 1:25284247a74c 11
sabazerefa 1:25284247a74c 12 #define BEZIER_ORDER_FOOT 7
lschwend 5:3d30627ae76e 13 #define NUM_INPUTS (27)
lschwend 5:3d30627ae76e 14 #define NUM_OUTPUTS 33
sabazerefa 1:25284247a74c 15
sabazerefa 1:25284247a74c 16 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
sabazerefa 1:25284247a74c 17
sabazerefa 1:25284247a74c 18 // Initializations
sabazerefa 1:25284247a74c 19 Serial pc(USBTX, USBRX); // USB Serial Terminal
sabazerefa 1:25284247a74c 20 ExperimentServer server; // Object that lets us communicate with MATLAB
sabazerefa 1:25284247a74c 21 Timer t; // Timer to measure elapsed time of experiment
sabazerefa 1:25284247a74c 22
sabazerefa 1:25284247a74c 23 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
sabazerefa 1:25284247a74c 24 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
sabazerefa 1:25284247a74c 25 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
sabazerefa 1:25284247a74c 26 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
sabazerefa 1:25284247a74c 27
sabazerefa 4:bb441c9325f4 28 float directionChange=1;//Not yet included properly!!
sabazerefa 1:25284247a74c 29 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
sabazerefa 1:25284247a74c 30 Ticker currentLoop;
sabazerefa 1:25284247a74c 31
sabazerefa 1:25284247a74c 32 // Variables for q1
sabazerefa 1:25284247a74c 33 float current1;
sabazerefa 1:25284247a74c 34 float current_des1 = 0;
sabazerefa 1:25284247a74c 35 float prev_current_des1 = 0;
sabazerefa 1:25284247a74c 36 float current_int1 = 0;
sabazerefa 1:25284247a74c 37 float angle1;
sabazerefa 1:25284247a74c 38 float velocity1;
sabazerefa 1:25284247a74c 39 float duty_cycle1;
sabazerefa 1:25284247a74c 40 float angle1_init;
sabazerefa 1:25284247a74c 41
sabazerefa 1:25284247a74c 42 // Variables for q2
sabazerefa 1:25284247a74c 43 float current2;
sabazerefa 1:25284247a74c 44 float current_des2 = 0;
sabazerefa 1:25284247a74c 45 float prev_current_des2 = 0;
sabazerefa 1:25284247a74c 46 float current_int2 = 0;
sabazerefa 1:25284247a74c 47 float angle2;
sabazerefa 1:25284247a74c 48 float velocity2;
sabazerefa 1:25284247a74c 49 float duty_cycle2;
sabazerefa 1:25284247a74c 50 float angle2_init;
sabazerefa 1:25284247a74c 51
sabazerefa 2:4e581e5b39e8 52 //Variables for q3 (leg 2 q1)
sabazerefa 2:4e581e5b39e8 53 float current3;
sabazerefa 2:4e581e5b39e8 54 float current_des3 = 0;
sabazerefa 2:4e581e5b39e8 55 float prev_current_des3 = 0;
sabazerefa 2:4e581e5b39e8 56 float current_int3 = 0;
sabazerefa 2:4e581e5b39e8 57 float angle3;
sabazerefa 2:4e581e5b39e8 58 float velocity3;
sabazerefa 2:4e581e5b39e8 59 float duty_cycle3;
sabazerefa 2:4e581e5b39e8 60 float angle3_init;
sabazerefa 2:4e581e5b39e8 61
sabazerefa 2:4e581e5b39e8 62 //variables for q4 (leg 2 q2)
sabazerefa 2:4e581e5b39e8 63 float current4;
sabazerefa 2:4e581e5b39e8 64 float current_des4 = 0;
sabazerefa 2:4e581e5b39e8 65 float prev_current_des4 = 0;
sabazerefa 2:4e581e5b39e8 66 float current_int4 = 0;
sabazerefa 2:4e581e5b39e8 67 float angle4;
sabazerefa 2:4e581e5b39e8 68 float velocity4;
sabazerefa 2:4e581e5b39e8 69 float duty_cycle4;
sabazerefa 2:4e581e5b39e8 70 float angle4_init;
sabazerefa 2:4e581e5b39e8 71
sabazerefa 1:25284247a74c 72 // Fixed kinematic parameters
sabazerefa 1:25284247a74c 73 const float l_OA=.011;
sabazerefa 1:25284247a74c 74 const float l_OB=.042;
sabazerefa 1:25284247a74c 75 const float l_AC=.096;
sabazerefa 1:25284247a74c 76 const float l_DE=.091;
sabazerefa 1:25284247a74c 77
sabazerefa 1:25284247a74c 78 // Timing parameters
sabazerefa 1:25284247a74c 79 float current_control_period_us = 200.0f; // 5kHz current control loop
sabazerefa 1:25284247a74c 80 float impedance_control_period_us = 1000.0f; // 1kHz impedance control loop
sabazerefa 1:25284247a74c 81 float start_period, traj_period, end_period;
sabazerefa 1:25284247a74c 82
sabazerefa 1:25284247a74c 83 // Control parameters
sabazerefa 1:25284247a74c 84 float current_Kp = 4.0f;
sabazerefa 1:25284247a74c 85 float current_Ki = 0.4f;
sabazerefa 1:25284247a74c 86 float current_int_max = 3.0f;
sabazerefa 1:25284247a74c 87 float duty_max;
sabazerefa 1:25284247a74c 88 float K_xx;
sabazerefa 1:25284247a74c 89 float K_yy;
sabazerefa 1:25284247a74c 90 float K_xy;
sabazerefa 1:25284247a74c 91 float D_xx;
sabazerefa 1:25284247a74c 92 float D_xy;
sabazerefa 1:25284247a74c 93 float D_yy;
sabazerefa 1:25284247a74c 94
sabazerefa 2:4e581e5b39e8 95
sabazerefa 2:4e581e5b39e8 96 //Second foot:
sabazerefa 2:4e581e5b39e8 97 float current_Kp2 = 4.0f;
sabazerefa 2:4e581e5b39e8 98 float current_Ki2 = 0.4f;
sabazerefa 2:4e581e5b39e8 99 float current_int_max2 = 3.0f;
sabazerefa 2:4e581e5b39e8 100 float duty_max2;
sabazerefa 2:4e581e5b39e8 101 float K_xx2;
sabazerefa 2:4e581e5b39e8 102 float K_yy2;
sabazerefa 2:4e581e5b39e8 103 float K_xy2;
sabazerefa 2:4e581e5b39e8 104 float D_xx2;
sabazerefa 2:4e581e5b39e8 105 float D_xy2;
sabazerefa 2:4e581e5b39e8 106 float D_yy2;
sabazerefa 2:4e581e5b39e8 107
sabazerefa 2:4e581e5b39e8 108
sabazerefa 1:25284247a74c 109 // Model parameters
sabazerefa 1:25284247a74c 110 float supply_voltage = 12; // motor supply voltage
sabazerefa 1:25284247a74c 111 float R = 2.0f; // motor resistance
sabazerefa 1:25284247a74c 112 float k_t = 0.18f; // motor torque constant
sabazerefa 1:25284247a74c 113 float nu = 0.0005; // motor viscous friction
sabazerefa 1:25284247a74c 114
lschwend 5:3d30627ae76e 115 // ellipse stuff
lschwend 5:3d30627ae76e 116 float y0;
lschwend 5:3d30627ae76e 117 float x0;
lschwend 5:3d30627ae76e 118 float Omega;
lschwend 5:3d30627ae76e 119 float a;
lschwend 5:3d30627ae76e 120 float b;
lschwend 5:3d30627ae76e 121 float phase;
lschwend 5:3d30627ae76e 122
sabazerefa 1:25284247a74c 123 // Current control interrupt function
sabazerefa 1:25284247a74c 124 void CurrentLoop()
sabazerefa 1:25284247a74c 125 {
sabazerefa 1:25284247a74c 126 // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
sabazerefa 1:25284247a74c 127
sabazerefa 1:25284247a74c 128 //use the motor shield as follows:
sabazerefa 1:25284247a74c 129 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
sabazerefa 1:25284247a74c 130
sabazerefa 1:25284247a74c 131 current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 132 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 133 float err_c1 = current_des1 - current1; // current errror
sabazerefa 1:25284247a74c 134 current_int1 += err_c1; // integrate error
sabazerefa 1:25284247a74c 135 current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 136 float ff1 = R*current_des1 + k_t*velocity1; // feedforward terms
sabazerefa 1:25284247a74c 137 duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 138
sabazerefa 1:25284247a74c 139 float absDuty1 = abs(duty_cycle1);
sabazerefa 1:25284247a74c 140 if (absDuty1 > duty_max) {
sabazerefa 1:25284247a74c 141 duty_cycle1 *= duty_max / absDuty1;
sabazerefa 1:25284247a74c 142 absDuty1 = duty_max;
sabazerefa 1:25284247a74c 143 }
sabazerefa 1:25284247a74c 144 if (duty_cycle1 < 0) { // backwards
sabazerefa 1:25284247a74c 145 motorShield.motorAWrite(absDuty1, 1);
sabazerefa 1:25284247a74c 146 } else { // forwards
sabazerefa 1:25284247a74c 147 motorShield.motorAWrite(absDuty1, 0);
sabazerefa 1:25284247a74c 148 }
sabazerefa 1:25284247a74c 149 prev_current_des1 = current_des1;
sabazerefa 1:25284247a74c 150
sabazerefa 1:25284247a74c 151 current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 152 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 153 float err_c2 = current_des2 - current2; // current error
sabazerefa 1:25284247a74c 154 current_int2 += err_c2; // integrate error
sabazerefa 1:25284247a74c 155 current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 156 float ff2 = R*current_des2 + k_t*velocity2; // feedforward terms
sabazerefa 1:25284247a74c 157 duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 158
sabazerefa 1:25284247a74c 159 float absDuty2 = abs(duty_cycle2);
sabazerefa 1:25284247a74c 160 if (absDuty2 > duty_max) {
sabazerefa 1:25284247a74c 161 duty_cycle2 *= duty_max / absDuty2;
sabazerefa 1:25284247a74c 162 absDuty2 = duty_max;
sabazerefa 1:25284247a74c 163 }
sabazerefa 1:25284247a74c 164 if (duty_cycle2 < 0) { // backwards
sabazerefa 1:25284247a74c 165 motorShield.motorBWrite(absDuty2, 1);
sabazerefa 1:25284247a74c 166 } else { // forwards
sabazerefa 1:25284247a74c 167 motorShield.motorBWrite(absDuty2, 0);
sabazerefa 1:25284247a74c 168 }
sabazerefa 1:25284247a74c 169 prev_current_des2 = current_des2;
sabazerefa 1:25284247a74c 170
sabazerefa 1:25284247a74c 171
sabazerefa 1:25284247a74c 172 current3 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 173 velocity3 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 174 float err_c3 = current_des3 - current3; // current errror
sabazerefa 1:25284247a74c 175 current_int3 += err_c3; // integrate error
sabazerefa 1:25284247a74c 176 current_int3 = fmaxf( fminf(current_int3, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 177 float ff3 = R*current_des3 + k_t*velocity3; // feedforward terms
sabazerefa 1:25284247a74c 178 duty_cycle3 = (ff3 + current_Kp*err_c3 + current_Ki*current_int3)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 179
sabazerefa 1:25284247a74c 180 float absDuty3 = abs(duty_cycle3);
sabazerefa 1:25284247a74c 181 if (absDuty3 > duty_max) {
sabazerefa 1:25284247a74c 182 duty_cycle3 *= duty_max / absDuty3;
sabazerefa 1:25284247a74c 183 absDuty3 = duty_max;
sabazerefa 1:25284247a74c 184 }
sabazerefa 1:25284247a74c 185 if (duty_cycle3 < 0) { // backwards
sabazerefa 1:25284247a74c 186 motorShield.motorCWrite(absDuty3, 1);
sabazerefa 1:25284247a74c 187 } else { // forwards
sabazerefa 1:25284247a74c 188 motorShield.motorCWrite(absDuty3, 0);
sabazerefa 1:25284247a74c 189 }
sabazerefa 1:25284247a74c 190 prev_current_des3 = current_des3;
sabazerefa 1:25284247a74c 191
sabazerefa 1:25284247a74c 192
sabazerefa 1:25284247a74c 193 current4 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 194 velocity4 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 195 float err_c4 = current_des4 - current4; // current errror
sabazerefa 1:25284247a74c 196 current_int4 += err_c4; // integrate error
sabazerefa 1:25284247a74c 197 current_int4 = fmaxf( fminf(current_int4, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 198 float ff4 = R*current_des4 + k_t*velocity4; // feedforward terms
sabazerefa 1:25284247a74c 199 duty_cycle4 = (ff4 + current_Kp*err_c4 + current_Ki*current_int4)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 200
sabazerefa 1:25284247a74c 201 float absDuty4 = abs(duty_cycle4);
sabazerefa 1:25284247a74c 202 if (absDuty4 > duty_max) {
sabazerefa 1:25284247a74c 203 duty_cycle4 *= duty_max / absDuty4;
sabazerefa 1:25284247a74c 204 absDuty4 = duty_max;
sabazerefa 1:25284247a74c 205 }
sabazerefa 1:25284247a74c 206 if (duty_cycle4 < 0) { // backwards
sabazerefa 1:25284247a74c 207 motorShield.motorCWrite(absDuty4, 1);
sabazerefa 1:25284247a74c 208 } else { // forwards
sabazerefa 1:25284247a74c 209 motorShield.motorCWrite(absDuty4, 0);
sabazerefa 1:25284247a74c 210 }
sabazerefa 1:25284247a74c 211 prev_current_des4 = current_des4;
sabazerefa 1:25284247a74c 212
sabazerefa 1:25284247a74c 213
sabazerefa 1:25284247a74c 214 }
sabazerefa 1:25284247a74c 215
sabazerefa 1:25284247a74c 216 int main (void)
sabazerefa 1:25284247a74c 217 {
sabazerefa 1:25284247a74c 218
sabazerefa 1:25284247a74c 219 // Object for 7th order Cartesian foot trajectory.
sabazerefa 1:25284247a74c 220
sabazerefa 1:25284247a74c 221 //CREATE A TRAJECTORY
lschwend 5:3d30627ae76e 222 //BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
sabazerefa 1:25284247a74c 223
sabazerefa 1:25284247a74c 224 // Link the terminal with our server and start it up
sabazerefa 1:25284247a74c 225 server.attachTerminal(pc);
sabazerefa 1:25284247a74c 226 server.init();
sabazerefa 1:25284247a74c 227
sabazerefa 1:25284247a74c 228 // Continually get input from MATLAB and run experiments
sabazerefa 1:25284247a74c 229 float input_params[NUM_INPUTS];
sabazerefa 1:25284247a74c 230 pc.printf("%f",input_params[0]);
sabazerefa 1:25284247a74c 231
sabazerefa 1:25284247a74c 232 while(1) {
sabazerefa 1:25284247a74c 233
sabazerefa 1:25284247a74c 234 // If there are new inputs, this code will run
sabazerefa 1:25284247a74c 235 if (server.getParams(input_params,NUM_INPUTS)) {
sabazerefa 1:25284247a74c 236
sabazerefa 1:25284247a74c 237
sabazerefa 1:25284247a74c 238 // Get inputs from MATLAB
sabazerefa 1:25284247a74c 239 start_period = input_params[0]; // First buffer time, before trajectory
sabazerefa 1:25284247a74c 240 traj_period = input_params[1]; // Trajectory time/length
sabazerefa 1:25284247a74c 241 end_period = input_params[2]; // Second buffer time, after trajectory
sabazerefa 1:25284247a74c 242
sabazerefa 1:25284247a74c 243 angle1_init = input_params[3]; // Initial angle for q1 (rad)
sabazerefa 1:25284247a74c 244 angle2_init = input_params[4]; // Initial angle for q2 (rad)
sabazerefa 1:25284247a74c 245
sabazerefa 1:25284247a74c 246 K_xx = input_params[5]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 247 K_yy = input_params[6]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 248 K_xy = input_params[7]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 249 D_xx = input_params[8]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 250 D_yy = input_params[9]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 251 D_xy = input_params[10]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 252 duty_max = input_params[11]; // Maximum duty factor
sabazerefa 2:4e581e5b39e8 253
sabazerefa 2:4e581e5b39e8 254 angle3_init = input_params[12];
sabazerefa 2:4e581e5b39e8 255 angle4_init = input_params[13];
sabazerefa 2:4e581e5b39e8 256
sabazerefa 2:4e581e5b39e8 257 K_xx2 = input_params[14];
sabazerefa 2:4e581e5b39e8 258 K_yy2 = input_params[15];
sabazerefa 2:4e581e5b39e8 259 K_xy2 = input_params[16];
sabazerefa 2:4e581e5b39e8 260 D_xx2 = input_params[17];
sabazerefa 2:4e581e5b39e8 261 D_yy2 = input_params[18];
sabazerefa 2:4e581e5b39e8 262 D_xy2 = input_params[19];
sabazerefa 3:2730130aa049 263 duty_max2 = input_params[20];
lschwend 5:3d30627ae76e 264
lschwend 5:3d30627ae76e 265 a = input_params[21];
lschwend 5:3d30627ae76e 266 b = input_params[22];
lschwend 5:3d30627ae76e 267 Omega = input_params[23];
lschwend 5:3d30627ae76e 268 y0 = input_params[24];
lschwend 5:3d30627ae76e 269 x0 = input_params[25];
lschwend 5:3d30627ae76e 270 phase = input_params[26];
lschwend 5:3d30627ae76e 271
sabazerefa 1:25284247a74c 272
sabazerefa 1:25284247a74c 273 // Get foot trajectory points
lschwend 5:3d30627ae76e 274 //float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
lschwend 5:3d30627ae76e 275 // for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
lschwend 5:3d30627ae76e 276 // foot_pts[i] = input_params[27+i];
lschwend 5:3d30627ae76e 277 // }
lschwend 5:3d30627ae76e 278 // rDesFoot_bez.setPoints(foot_pts);
sabazerefa 1:25284247a74c 279
sabazerefa 1:25284247a74c 280 // Attach current loop interrupt
sabazerefa 1:25284247a74c 281 currentLoop.attach_us(CurrentLoop,current_control_period_us);
sabazerefa 1:25284247a74c 282
sabazerefa 1:25284247a74c 283 // Setup experiment
sabazerefa 1:25284247a74c 284 t.reset();
sabazerefa 1:25284247a74c 285 t.start();
sabazerefa 1:25284247a74c 286 encoderA.reset();
sabazerefa 1:25284247a74c 287 encoderB.reset();
sabazerefa 1:25284247a74c 288 encoderC.reset();
sabazerefa 1:25284247a74c 289 encoderD.reset();
sabazerefa 1:25284247a74c 290
sabazerefa 1:25284247a74c 291 motorShield.motorAWrite(0, 0); //turn motor A off
sabazerefa 1:25284247a74c 292 motorShield.motorBWrite(0, 0); //turn motor B off
sabazerefa 2:4e581e5b39e8 293 motorShield.motorCWrite(0, 0);
sabazerefa 2:4e581e5b39e8 294 motorShield.motorDWrite(0, 0);
sabazerefa 1:25284247a74c 295
sabazerefa 1:25284247a74c 296 // Run experiment
sabazerefa 1:25284247a74c 297 while( t.read() < start_period + traj_period + end_period) {
sabazerefa 1:25284247a74c 298
sabazerefa 1:25284247a74c 299 // Read encoders to get motor states
sabazerefa 1:25284247a74c 300 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
sabazerefa 1:25284247a74c 301 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
sabazerefa 1:25284247a74c 302
sabazerefa 1:25284247a74c 303 angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;
sabazerefa 2:4e581e5b39e8 304 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
sabazerefa 2:4e581e5b39e8 305
sabazerefa 2:4e581e5b39e8 306
sabazerefa 2:4e581e5b39e8 307 angle3 = encoderC.getPulses() *PULSE_TO_RAD + angle3_init;
sabazerefa 2:4e581e5b39e8 308 velocity3 = encoderC.getVelocity() * PULSE_TO_RAD;
sabazerefa 2:4e581e5b39e8 309
sabazerefa 2:4e581e5b39e8 310 angle4 = encoderD.getPulses() * PULSE_TO_RAD + angle4_init;
sabazerefa 2:4e581e5b39e8 311 velocity4 = encoderD.getVelocity() * PULSE_TO_RAD;
sabazerefa 2:4e581e5b39e8 312
sabazerefa 1:25284247a74c 313
sabazerefa 1:25284247a74c 314 const float th1 = angle1;
sabazerefa 1:25284247a74c 315 const float th2 = angle2;
sabazerefa 1:25284247a74c 316 const float dth1= velocity1;
sabazerefa 1:25284247a74c 317 const float dth2= velocity2;
sabazerefa 1:25284247a74c 318
sabazerefa 2:4e581e5b39e8 319
lschwend 5:3d30627ae76e 320 const float th3 = -angle3;
lschwend 5:3d30627ae76e 321 const float th4 = -angle4;
lschwend 5:3d30627ae76e 322 const float dth3= -velocity3;
lschwend 5:3d30627ae76e 323 const float dth4= -velocity4;
sabazerefa 2:4e581e5b39e8 324
sabazerefa 2:4e581e5b39e8 325
sabazerefa 2:4e581e5b39e8 326
sabazerefa 1:25284247a74c 327 // Calculate the Jacobian
sabazerefa 1:25284247a74c 328 float Jx_th1 = l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1);
sabazerefa 1:25284247a74c 329 float Jx_th2 = l_AC*cos(th1+th2);
sabazerefa 1:25284247a74c 330 float Jy_th1 = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
sabazerefa 1:25284247a74c 331 float Jy_th2 = l_AC*sin(th1+th2);
sabazerefa 2:4e581e5b39e8 332
sabazerefa 2:4e581e5b39e8 333
sabazerefa 2:4e581e5b39e8 334 float Jx_th3 = l_AC*cos(th3+th4)+l_DE*cos(th3)+l_OB*cos(th3);
sabazerefa 2:4e581e5b39e8 335 float Jx_th4 = l_AC*cos(th3+th4);
sabazerefa 2:4e581e5b39e8 336 float Jy_th3 = l_AC*sin(th3+th4)+l_DE*sin(th3)+l_OB*sin(th3);
sabazerefa 2:4e581e5b39e8 337 float Jy_th4 = l_AC*sin(th3+th4);
sabazerefa 1:25284247a74c 338
sabazerefa 1:25284247a74c 339
sabazerefa 1:25284247a74c 340 // Calculate the forward kinematics (position and velocity)
sabazerefa 1:25284247a74c 341 float xFoot = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
sabazerefa 1:25284247a74c 342 float yFoot = -l_AC*cos(th1+th2)-l_DE*cos(th1)-l_OB*cos(th1);
sabazerefa 1:25284247a74c 343 float dxFoot = Jx_th1*dth1+Jx_th2*dth2;
sabazerefa 2:4e581e5b39e8 344 float dyFoot = Jy_th1*dth1+Jy_th2*dth2;
sabazerefa 2:4e581e5b39e8 345
sabazerefa 2:4e581e5b39e8 346
sabazerefa 2:4e581e5b39e8 347 float xFoot2 = l_AC*sin(th3+th4)+l_DE*sin(th3)+l_OB*sin(th3);
sabazerefa 2:4e581e5b39e8 348 float yFoot2 = -l_AC*cos(th3+th4)-l_DE*cos(th3)-l_OB*cos(th3);
sabazerefa 2:4e581e5b39e8 349 float dxFoot2 = Jx_th3*dth3+Jx_th4*dth4;
sabazerefa 2:4e581e5b39e8 350 float dyFoot2 = Jy_th3*dth3+Jy_th4*dth4;
sabazerefa 2:4e581e5b39e8 351
sabazerefa 1:25284247a74c 352
sabazerefa 1:25284247a74c 353 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
sabazerefa 1:25284247a74c 354 float teff = 0;
sabazerefa 1:25284247a74c 355 float vMult = 0;
sabazerefa 1:25284247a74c 356 if( t < start_period) {
sabazerefa 2:4e581e5b39e8 357 if (K_xx > 0 || K_yy > 0 || K_xx2 > 0 || K_yy2 >0) {
sabazerefa 1:25284247a74c 358 K_xx = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
sabazerefa 1:25284247a74c 359 K_yy = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
sabazerefa 1:25284247a74c 360 D_xx = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
sabazerefa 1:25284247a74c 361 D_yy = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
sabazerefa 1:25284247a74c 362 K_xy = 0;
sabazerefa 1:25284247a74c 363 D_xy = 0;
sabazerefa 2:4e581e5b39e8 364
sabazerefa 2:4e581e5b39e8 365 K_xx2=1;
sabazerefa 2:4e581e5b39e8 366 K_yy2=1;
sabazerefa 2:4e581e5b39e8 367 D_xx2=0.1;
sabazerefa 2:4e581e5b39e8 368 D_yy2=0.1;
sabazerefa 2:4e581e5b39e8 369 D_xy2=0;
sabazerefa 2:4e581e5b39e8 370 K_xy2=0;
sabazerefa 2:4e581e5b39e8 371
sabazerefa 2:4e581e5b39e8 372
sabazerefa 1:25284247a74c 373 }
sabazerefa 1:25284247a74c 374 teff = 0;
sabazerefa 1:25284247a74c 375 }
sabazerefa 1:25284247a74c 376 else if (t < start_period + traj_period)
sabazerefa 1:25284247a74c 377 {
sabazerefa 1:25284247a74c 378 K_xx = input_params[5]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 379 K_yy = input_params[6]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 380 K_xy = input_params[7]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 381 D_xx = input_params[8]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 382 D_yy = input_params[9]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 383 D_xy = input_params[10]; // Foot damping N/(m/s)
sabazerefa 2:4e581e5b39e8 384
sabazerefa 2:4e581e5b39e8 385 K_xx2 = input_params[14];
sabazerefa 2:4e581e5b39e8 386 K_yy2 = input_params[15];
sabazerefa 2:4e581e5b39e8 387 K_xy2 = input_params[16];
sabazerefa 2:4e581e5b39e8 388 D_xx2 = input_params[17];
sabazerefa 2:4e581e5b39e8 389 D_yy2 = input_params[18];
sabazerefa 2:4e581e5b39e8 390 D_xy2 = input_params[19];
sabazerefa 2:4e581e5b39e8 391
sabazerefa 2:4e581e5b39e8 392
sabazerefa 1:25284247a74c 393 teff = (t-start_period);
sabazerefa 1:25284247a74c 394 vMult = 1;
sabazerefa 1:25284247a74c 395 }
sabazerefa 1:25284247a74c 396 else
sabazerefa 1:25284247a74c 397 {
sabazerefa 1:25284247a74c 398 teff = traj_period;
sabazerefa 1:25284247a74c 399 vMult = 0;
sabazerefa 1:25284247a74c 400 }
sabazerefa 1:25284247a74c 401
sabazerefa 1:25284247a74c 402 // Get desired foot positions and velocities
lschwend 5:3d30627ae76e 403 //float rDesFoot[2] , vDesFoot[2];
lschwend 5:3d30627ae76e 404 // rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
lschwend 5:3d30627ae76e 405 // rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
lschwend 5:3d30627ae76e 406 // vDesFoot[0]/=traj_period;
lschwend 5:3d30627ae76e 407 // vDesFoot[1]/=traj_period;
lschwend 5:3d30627ae76e 408 // vDesFoot[0]*=vMult;
lschwend 5:3d30627ae76e 409 // vDesFoot[1]*=vMult;
sabazerefa 1:25284247a74c 410
lschwend 5:3d30627ae76e 411 // float rDesFoot2[2] , vDesFoot2[2];
lschwend 5:3d30627ae76e 412 // float evalPoint = teff/traj_period+traj_period/2;
lschwend 5:3d30627ae76e 413 // if(evalPoint>traj_period) evalPoint=evalPoint-traj_period;
lschwend 5:3d30627ae76e 414 // rDesFoot_bez.evaluate(evalPoint,rDesFoot2);
lschwend 5:3d30627ae76e 415 // rDesFoot_bez.evaluateDerivative(evalPoint,vDesFoot2);
lschwend 5:3d30627ae76e 416 // vDesFoot2[0]/=traj_period;
lschwend 5:3d30627ae76e 417 // vDesFoot2[1]/=traj_period;
lschwend 5:3d30627ae76e 418 // vDesFoot2[0]*=vMult;
lschwend 5:3d30627ae76e 419 // vDesFoot2[1]*=vMult;
sabazerefa 2:4e581e5b39e8 420
sabazerefa 1:25284247a74c 421 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
lschwend 5:3d30627ae76e 422 //float xFoot_inv = -rDesFoot[0];
lschwend 5:3d30627ae76e 423 // float yFoot_inv = rDesFoot[1];
lschwend 5:3d30627ae76e 424 // float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
lschwend 5:3d30627ae76e 425 // 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)) ));
lschwend 5:3d30627ae76e 426 // float th2_des = -(3.14159f - alpha);
lschwend 5:3d30627ae76e 427 // float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
lschwend 5:3d30627ae76e 428 //
lschwend 5:3d30627ae76e 429 //
lschwend 5:3d30627ae76e 430 // float xFoot_inv2 = -rDesFoot2[0];
lschwend 5:3d30627ae76e 431 // float yFoot_inv2 = rDesFoot2[1];
lschwend 5:3d30627ae76e 432 // float alpha2 = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
lschwend 5:3d30627ae76e 433 // float th3_des = -(3.14159f - alpha2);
lschwend 5:3d30627ae76e 434 // float th4_des = -((3.14159f/2.0f) + atan2(yFoot_inv2,xFoot_inv2) - abs(asin( (l_AC/l_OE)*sin(alpha2) )));
lschwend 5:3d30627ae76e 435 //
lschwend 5:3d30627ae76e 436 //
lschwend 5:3d30627ae76e 437 //
lschwend 5:3d30627ae76e 438 // float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
lschwend 5:3d30627ae76e 439 // float dth1_des = (1.0f/dd) * ( Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
lschwend 5:3d30627ae76e 440 // float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
lschwend 5:3d30627ae76e 441 //
lschwend 5:3d30627ae76e 442 // float dd2 = (Jx_th3*Jy_th4 - Jx_th4*Jy_th3);
lschwend 5:3d30627ae76e 443 // float dth3_des = (1.0f/dd2) * ( Jy_th4*vDesFoot[0] - Jx_th4*vDesFoot[1] );
lschwend 5:3d30627ae76e 444 // float dth4_des = (1.0f/dd2) * ( -Jy_th3*vDesFoot[0] + Jx_th3*vDesFoot[1] );
lschwend 5:3d30627ae76e 445
lschwend 5:3d30627ae76e 446 // Calculate error variables and the desired position in ellispe
lschwend 5:3d30627ae76e 447 float xDes = b*cos(Omega*teff) + x0;
lschwend 5:3d30627ae76e 448 float yDes = a*sin(Omega*teff) + y0;
lschwend 5:3d30627ae76e 449 float vxDes = 0;//-b*Omega*sin(Omega*teff);
lschwend 5:3d30627ae76e 450 float vyDes = 0; //a*Omega*cos(Omega*teff);
sabazerefa 2:4e581e5b39e8 451
lschwend 5:3d30627ae76e 452 float e_x = xDes - xFoot;
lschwend 5:3d30627ae76e 453 float e_y = yDes - yFoot;
lschwend 5:3d30627ae76e 454 float de_x = vxDes - dxFoot;
lschwend 5:3d30627ae76e 455 float de_y = vyDes - dyFoot;
sabazerefa 2:4e581e5b39e8 456
lschwend 5:3d30627ae76e 457 float xDes2 = b*cos(Omega*teff + phase) + x0;
lschwend 5:3d30627ae76e 458 float yDes2 = a*sin(Omega*teff + phase) + y0;
lschwend 5:3d30627ae76e 459 float vxDes2 = 0;//-b*Omega*sin(Omega*teff + phase);
lschwend 5:3d30627ae76e 460 float vyDes2 = 0; //a*Omega*cos(Omega*teff + phase);
sabazerefa 2:4e581e5b39e8 461
lschwend 5:3d30627ae76e 462 float e_x2 = xDes2 - xFoot2;
lschwend 5:3d30627ae76e 463 float e_y2 = yDes2 - yFoot2;
lschwend 5:3d30627ae76e 464 float de_x2 = vxDes2 - dxFoot2;
lschwend 5:3d30627ae76e 465 float de_y2 = vyDes2 - dyFoot2;
lschwend 5:3d30627ae76e 466
lschwend 5:3d30627ae76e 467
sabazerefa 1:25284247a74c 468
sabazerefa 1:25284247a74c 469 // Calculate virtual force on foot
lschwend 5:3d30627ae76e 470 float fx = K_xx*(e_x) +K_xy*(e_y)+D_xx*(de_x)+D_xy*(de_y);
lschwend 5:3d30627ae76e 471 float fy = K_xy*(e_x) + K_yy*(e_y) + D_xy*(de_x)+D_yy*(de_y);
sabazerefa 2:4e581e5b39e8 472
lschwend 5:3d30627ae76e 473 float fx2 = K_xx2*(e_x2) +K_xy2*(e_y2)+D_xx2*(de_x2)+D_xy2*(de_y2);
lschwend 5:3d30627ae76e 474 float fy2 = K_xy2*(e_x2) + K_yy2*(e_y2) + D_xy2*(de_x2)+D_yy2*(de_y2);
sabazerefa 2:4e581e5b39e8 475
sabazerefa 2:4e581e5b39e8 476
sabazerefa 1:25284247a74c 477 // Set desired currents
sabazerefa 1:25284247a74c 478 current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;
sabazerefa 1:25284247a74c 479 current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;
lschwend 5:3d30627ae76e 480 current_des3 = (Jx_th1*fx2+Jy_th1*fy2)/k_t;
lschwend 5:3d30627ae76e 481 current_des4 = (Jx_th2*fx2+Jy_th2*fy2)/k_t;
sabazerefa 1:25284247a74c 482
sabazerefa 1:25284247a74c 483
sabazerefa 1:25284247a74c 484
sabazerefa 1:25284247a74c 485 // Joint impedance
sabazerefa 1:25284247a74c 486 // sub Kxx for K1, Dxx for D1, Kyy for K2, Dyy for D2
sabazerefa 1:25284247a74c 487 // Note: Be careful with signs now that you have non-zero desired angles!
sabazerefa 1:25284247a74c 488 // Your equations should be of the form i_d = K1*(q1_d - q1) + D1*(dq1_d - dq1)
sabazerefa 1:25284247a74c 489
sabazerefa 1:25284247a74c 490 //PART 0A FIRST
sabazerefa 1:25284247a74c 491 // float q1_d=0;
sabazerefa 1:25284247a74c 492 // float dq1_d=0;
sabazerefa 1:25284247a74c 493 // current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;
sabazerefa 1:25284247a74c 494 // current_des2 = 0;
sabazerefa 1:25284247a74c 495
sabazerefa 1:25284247a74c 496 // PART 2
sabazerefa 1:25284247a74c 497 // float q1_d=th1_des;
sabazerefa 1:25284247a74c 498 // float dq1_d=dth1_des;
sabazerefa 1:25284247a74c 499 // float q2_d=th2_des;
sabazerefa 1:25284247a74c 500 // float dq2_d=dth2_des;
sabazerefa 1:25284247a74c 501 // current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;
sabazerefa 1:25284247a74c 502 // current_des2 = (K_yy*(q2_d-th2) + D_yy*(dq2_d-dth2))/k_t;
sabazerefa 1:25284247a74c 503
sabazerefa 1:25284247a74c 504
sabazerefa 1:25284247a74c 505
sabazerefa 1:25284247a74c 506 /* PART 3!!!!!!!!!!!!!!!!*/
sabazerefa 1:25284247a74c 507
sabazerefa 1:25284247a74c 508 // Cartesian impedance
sabazerefa 1:25284247a74c 509 // Note: As with the joint space laws, be careful with signs!
sabazerefa 1:25284247a74c 510 // current_des1 = 0;
sabazerefa 1:25284247a74c 511 // current_des2 = 0;
sabazerefa 1:25284247a74c 512
sabazerefa 1:25284247a74c 513
sabazerefa 1:25284247a74c 514 // Form output to send to MATLAB
sabazerefa 1:25284247a74c 515 float output_data[NUM_OUTPUTS];
sabazerefa 1:25284247a74c 516 // current time
sabazerefa 1:25284247a74c 517 output_data[0] = t.read();
sabazerefa 1:25284247a74c 518 // motor 1 state
sabazerefa 1:25284247a74c 519 output_data[1] = angle1;
sabazerefa 1:25284247a74c 520 output_data[2] = velocity1;
sabazerefa 1:25284247a74c 521 output_data[3] = current1;
sabazerefa 1:25284247a74c 522 output_data[4] = current_des1;
sabazerefa 1:25284247a74c 523 output_data[5] = duty_cycle1;
sabazerefa 1:25284247a74c 524 // motor 2 state
sabazerefa 1:25284247a74c 525 output_data[6] = angle2;
sabazerefa 1:25284247a74c 526 output_data[7] = velocity2;
sabazerefa 1:25284247a74c 527 output_data[8] = current2;
sabazerefa 1:25284247a74c 528 output_data[9] = current_des2;
sabazerefa 1:25284247a74c 529 output_data[10]= duty_cycle2;
sabazerefa 2:4e581e5b39e8 530 // motor 3 state
lschwend 5:3d30627ae76e 531 output_data[11] = angle3;
lschwend 5:3d30627ae76e 532 output_data[12] = velocity3;
lschwend 5:3d30627ae76e 533 output_data[13] = current3;
lschwend 5:3d30627ae76e 534 output_data[14] = current_des3;
lschwend 5:3d30627ae76e 535 output_data[15]= duty_cycle3;
sabazerefa 2:4e581e5b39e8 536 // motor 4 state
lschwend 5:3d30627ae76e 537 output_data[16] = angle4;
lschwend 5:3d30627ae76e 538 output_data[17] = velocity4;
lschwend 5:3d30627ae76e 539 output_data[18] = current4;
lschwend 5:3d30627ae76e 540 output_data[19] = current_des4;
lschwend 5:3d30627ae76e 541 output_data[20]= duty_cycle4;
sabazerefa 1:25284247a74c 542 // foot state
sabazerefa 2:4e581e5b39e8 543 output_data[21] = xFoot;
sabazerefa 2:4e581e5b39e8 544 output_data[22] = yFoot;
sabazerefa 2:4e581e5b39e8 545 output_data[23] = dxFoot;
sabazerefa 2:4e581e5b39e8 546 output_data[24] = dyFoot;
lschwend 5:3d30627ae76e 547 output_data[25] = xDes;
lschwend 5:3d30627ae76e 548 output_data[26] = yDes;
lschwend 5:3d30627ae76e 549 output_data[27] = vxDes;
lschwend 5:3d30627ae76e 550 output_data[28] = vyDes;
lschwend 5:3d30627ae76e 551
lschwend 5:3d30627ae76e 552 output_data[29] = xFoot2;
lschwend 5:3d30627ae76e 553 output_data[30] = yFoot2;
lschwend 5:3d30627ae76e 554 output_data[31] = xDes2;
lschwend 5:3d30627ae76e 555 output_data[32] = yDes2;
lschwend 5:3d30627ae76e 556
sabazerefa 1:25284247a74c 557
sabazerefa 1:25284247a74c 558 // Send data to MATLAB
sabazerefa 1:25284247a74c 559 server.sendData(output_data,NUM_OUTPUTS);
sabazerefa 1:25284247a74c 560
sabazerefa 1:25284247a74c 561 wait_us(impedance_control_period_us);
sabazerefa 1:25284247a74c 562 }
sabazerefa 1:25284247a74c 563
sabazerefa 1:25284247a74c 564 // Cleanup after experiment
sabazerefa 1:25284247a74c 565 server.setExperimentComplete();
sabazerefa 1:25284247a74c 566 currentLoop.detach();
sabazerefa 1:25284247a74c 567 motorShield.motorAWrite(0, 0); //turn motor A off
sabazerefa 1:25284247a74c 568 motorShield.motorBWrite(0, 0); //turn motor B off
sabazerefa 1:25284247a74c 569 motorShield.motorCWrite(0,0);
sabazerefa 1:25284247a74c 570 motorShield.motorDWrite(0,0);
sabazerefa 1:25284247a74c 571
sabazerefa 1:25284247a74c 572 } // end if
sabazerefa 1:25284247a74c 573
sabazerefa 1:25284247a74c 574 } // end while
sabazerefa 1:25284247a74c 575
sabazerefa 1:25284247a74c 576 } // end main
sabazerefa 1:25284247a74c 577