Laura Schwendeman / Mbed OS 274TeamProject2

als;djfpoafb hnw3jg

Dependencies:   MatrixMath Matrix ExperimentServer QEI_pmw MotorShield

main.cpp@3:2730130aa049, 2021-11-22 (annotated)

Committer:
sabazerefa
Date:
Mon Nov 22 20:40:44 2021 +0000
Revision:
3:2730130aa049
Parent:
2:4e581e5b39e8
Child:
4:bb441c9325f4
Compiles

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
sabazerefa 2:4e581e5b39e8 13 #define NUM_INPUTS (21 + 2*(BEZIER_ORDER_FOOT+1))
sabazerefa 2:4e581e5b39e8 14 #define NUM_OUTPUTS 29
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 2:4e581e5b39e8 28 float directionChange=-1;
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
sabazerefa 1:25284247a74c 115 // Current control interrupt function
sabazerefa 1:25284247a74c 116 void CurrentLoop()
sabazerefa 1:25284247a74c 117 {
sabazerefa 1:25284247a74c 118 // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
sabazerefa 1:25284247a74c 119
sabazerefa 1:25284247a74c 120 //use the motor shield as follows:
sabazerefa 1:25284247a74c 121 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
sabazerefa 1:25284247a74c 122
sabazerefa 1:25284247a74c 123 current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 124 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 125 float err_c1 = current_des1 - current1; // current errror
sabazerefa 1:25284247a74c 126 current_int1 += err_c1; // integrate error
sabazerefa 1:25284247a74c 127 current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 128 float ff1 = R*current_des1 + k_t*velocity1; // feedforward terms
sabazerefa 1:25284247a74c 129 duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 130
sabazerefa 1:25284247a74c 131 float absDuty1 = abs(duty_cycle1);
sabazerefa 1:25284247a74c 132 if (absDuty1 > duty_max) {
sabazerefa 1:25284247a74c 133 duty_cycle1 *= duty_max / absDuty1;
sabazerefa 1:25284247a74c 134 absDuty1 = duty_max;
sabazerefa 1:25284247a74c 135 }
sabazerefa 1:25284247a74c 136 if (duty_cycle1 < 0) { // backwards
sabazerefa 1:25284247a74c 137 motorShield.motorAWrite(absDuty1, 1);
sabazerefa 1:25284247a74c 138 } else { // forwards
sabazerefa 1:25284247a74c 139 motorShield.motorAWrite(absDuty1, 0);
sabazerefa 1:25284247a74c 140 }
sabazerefa 1:25284247a74c 141 prev_current_des1 = current_des1;
sabazerefa 1:25284247a74c 142
sabazerefa 1:25284247a74c 143 current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 144 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 145 float err_c2 = current_des2 - current2; // current error
sabazerefa 1:25284247a74c 146 current_int2 += err_c2; // integrate error
sabazerefa 1:25284247a74c 147 current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 148 float ff2 = R*current_des2 + k_t*velocity2; // feedforward terms
sabazerefa 1:25284247a74c 149 duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 150
sabazerefa 1:25284247a74c 151 float absDuty2 = abs(duty_cycle2);
sabazerefa 1:25284247a74c 152 if (absDuty2 > duty_max) {
sabazerefa 1:25284247a74c 153 duty_cycle2 *= duty_max / absDuty2;
sabazerefa 1:25284247a74c 154 absDuty2 = duty_max;
sabazerefa 1:25284247a74c 155 }
sabazerefa 1:25284247a74c 156 if (duty_cycle2 < 0) { // backwards
sabazerefa 1:25284247a74c 157 motorShield.motorBWrite(absDuty2, 1);
sabazerefa 1:25284247a74c 158 } else { // forwards
sabazerefa 1:25284247a74c 159 motorShield.motorBWrite(absDuty2, 0);
sabazerefa 1:25284247a74c 160 }
sabazerefa 1:25284247a74c 161 prev_current_des2 = current_des2;
sabazerefa 1:25284247a74c 162
sabazerefa 1:25284247a74c 163
sabazerefa 1:25284247a74c 164 current3 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 165 velocity3 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 166 float err_c3 = current_des3 - current3; // current errror
sabazerefa 1:25284247a74c 167 current_int3 += err_c3; // integrate error
sabazerefa 1:25284247a74c 168 current_int3 = fmaxf( fminf(current_int3, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 169 float ff3 = R*current_des3 + k_t*velocity3; // feedforward terms
sabazerefa 1:25284247a74c 170 duty_cycle3 = (ff3 + current_Kp*err_c3 + current_Ki*current_int3)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 171
sabazerefa 1:25284247a74c 172 float absDuty3 = abs(duty_cycle3);
sabazerefa 1:25284247a74c 173 if (absDuty3 > duty_max) {
sabazerefa 1:25284247a74c 174 duty_cycle3 *= duty_max / absDuty3;
sabazerefa 1:25284247a74c 175 absDuty3 = duty_max;
sabazerefa 1:25284247a74c 176 }
sabazerefa 1:25284247a74c 177 if (duty_cycle3 < 0) { // backwards
sabazerefa 1:25284247a74c 178 motorShield.motorCWrite(absDuty3, 1);
sabazerefa 1:25284247a74c 179 } else { // forwards
sabazerefa 1:25284247a74c 180 motorShield.motorCWrite(absDuty3, 0);
sabazerefa 1:25284247a74c 181 }
sabazerefa 1:25284247a74c 182 prev_current_des3 = current_des3;
sabazerefa 1:25284247a74c 183
sabazerefa 1:25284247a74c 184
sabazerefa 1:25284247a74c 185 current4 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 186 velocity4 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 187 float err_c4 = current_des4 - current4; // current errror
sabazerefa 1:25284247a74c 188 current_int4 += err_c4; // integrate error
sabazerefa 1:25284247a74c 189 current_int4 = fmaxf( fminf(current_int4, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 190 float ff4 = R*current_des4 + k_t*velocity4; // feedforward terms
sabazerefa 1:25284247a74c 191 duty_cycle4 = (ff4 + current_Kp*err_c4 + current_Ki*current_int4)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 192
sabazerefa 1:25284247a74c 193 float absDuty4 = abs(duty_cycle4);
sabazerefa 1:25284247a74c 194 if (absDuty4 > duty_max) {
sabazerefa 1:25284247a74c 195 duty_cycle4 *= duty_max / absDuty4;
sabazerefa 1:25284247a74c 196 absDuty4 = duty_max;
sabazerefa 1:25284247a74c 197 }
sabazerefa 1:25284247a74c 198 if (duty_cycle4 < 0) { // backwards
sabazerefa 1:25284247a74c 199 motorShield.motorCWrite(absDuty4, 1);
sabazerefa 1:25284247a74c 200 } else { // forwards
sabazerefa 1:25284247a74c 201 motorShield.motorCWrite(absDuty4, 0);
sabazerefa 1:25284247a74c 202 }
sabazerefa 1:25284247a74c 203 prev_current_des4 = current_des4;
sabazerefa 1:25284247a74c 204
sabazerefa 1:25284247a74c 205
sabazerefa 1:25284247a74c 206 }
sabazerefa 1:25284247a74c 207
sabazerefa 1:25284247a74c 208 int main (void)
sabazerefa 1:25284247a74c 209 {
sabazerefa 1:25284247a74c 210
sabazerefa 1:25284247a74c 211 // Object for 7th order Cartesian foot trajectory.
sabazerefa 1:25284247a74c 212
sabazerefa 1:25284247a74c 213 //CREATE A TRAJECTORY
sabazerefa 1:25284247a74c 214 BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
sabazerefa 1:25284247a74c 215
sabazerefa 1:25284247a74c 216 // Link the terminal with our server and start it up
sabazerefa 1:25284247a74c 217 server.attachTerminal(pc);
sabazerefa 1:25284247a74c 218 server.init();
sabazerefa 1:25284247a74c 219
sabazerefa 1:25284247a74c 220 // Continually get input from MATLAB and run experiments
sabazerefa 1:25284247a74c 221 float input_params[NUM_INPUTS];
sabazerefa 1:25284247a74c 222 pc.printf("%f",input_params[0]);
sabazerefa 1:25284247a74c 223
sabazerefa 1:25284247a74c 224 while(1) {
sabazerefa 1:25284247a74c 225
sabazerefa 1:25284247a74c 226 // If there are new inputs, this code will run
sabazerefa 1:25284247a74c 227 if (server.getParams(input_params,NUM_INPUTS)) {
sabazerefa 1:25284247a74c 228
sabazerefa 1:25284247a74c 229
sabazerefa 1:25284247a74c 230 // Get inputs from MATLAB
sabazerefa 1:25284247a74c 231 start_period = input_params[0]; // First buffer time, before trajectory
sabazerefa 1:25284247a74c 232 traj_period = input_params[1]; // Trajectory time/length
sabazerefa 1:25284247a74c 233 end_period = input_params[2]; // Second buffer time, after trajectory
sabazerefa 1:25284247a74c 234
sabazerefa 1:25284247a74c 235 angle1_init = input_params[3]; // Initial angle for q1 (rad)
sabazerefa 1:25284247a74c 236 angle2_init = input_params[4]; // Initial angle for q2 (rad)
sabazerefa 1:25284247a74c 237
sabazerefa 1:25284247a74c 238 K_xx = input_params[5]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 239 K_yy = input_params[6]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 240 K_xy = input_params[7]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 241 D_xx = input_params[8]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 242 D_yy = input_params[9]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 243 D_xy = input_params[10]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 244 duty_max = input_params[11]; // Maximum duty factor
sabazerefa 2:4e581e5b39e8 245
sabazerefa 2:4e581e5b39e8 246 angle3_init = input_params[12];
sabazerefa 2:4e581e5b39e8 247 angle4_init = input_params[13];
sabazerefa 2:4e581e5b39e8 248
sabazerefa 2:4e581e5b39e8 249 K_xx2 = input_params[14];
sabazerefa 2:4e581e5b39e8 250 K_yy2 = input_params[15];
sabazerefa 2:4e581e5b39e8 251 K_xy2 = input_params[16];
sabazerefa 2:4e581e5b39e8 252 D_xx2 = input_params[17];
sabazerefa 2:4e581e5b39e8 253 D_yy2 = input_params[18];
sabazerefa 2:4e581e5b39e8 254 D_xy2 = input_params[19];
sabazerefa 3:2730130aa049 255 duty_max2 = input_params[20];
sabazerefa 1:25284247a74c 256
sabazerefa 1:25284247a74c 257 // Get foot trajectory points
sabazerefa 1:25284247a74c 258 float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
sabazerefa 1:25284247a74c 259 for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
sabazerefa 2:4e581e5b39e8 260 foot_pts[i] = input_params[21+i];
sabazerefa 1:25284247a74c 261 }
sabazerefa 1:25284247a74c 262 rDesFoot_bez.setPoints(foot_pts);
sabazerefa 1:25284247a74c 263
sabazerefa 1:25284247a74c 264 // Attach current loop interrupt
sabazerefa 1:25284247a74c 265 currentLoop.attach_us(CurrentLoop,current_control_period_us);
sabazerefa 1:25284247a74c 266
sabazerefa 1:25284247a74c 267 // Setup experiment
sabazerefa 1:25284247a74c 268 t.reset();
sabazerefa 1:25284247a74c 269 t.start();
sabazerefa 1:25284247a74c 270 encoderA.reset();
sabazerefa 1:25284247a74c 271 encoderB.reset();
sabazerefa 1:25284247a74c 272 encoderC.reset();
sabazerefa 1:25284247a74c 273 encoderD.reset();
sabazerefa 1:25284247a74c 274
sabazerefa 1:25284247a74c 275 motorShield.motorAWrite(0, 0); //turn motor A off
sabazerefa 1:25284247a74c 276 motorShield.motorBWrite(0, 0); //turn motor B off
sabazerefa 2:4e581e5b39e8 277 motorShield.motorCWrite(0, 0);
sabazerefa 2:4e581e5b39e8 278 motorShield.motorDWrite(0, 0);
sabazerefa 1:25284247a74c 279
sabazerefa 1:25284247a74c 280 // Run experiment
sabazerefa 1:25284247a74c 281 while( t.read() < start_period + traj_period + end_period) {
sabazerefa 1:25284247a74c 282
sabazerefa 1:25284247a74c 283 // Read encoders to get motor states
sabazerefa 1:25284247a74c 284 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
sabazerefa 1:25284247a74c 285 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
sabazerefa 1:25284247a74c 286
sabazerefa 1:25284247a74c 287 angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;
sabazerefa 2:4e581e5b39e8 288 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
sabazerefa 2:4e581e5b39e8 289
sabazerefa 2:4e581e5b39e8 290
sabazerefa 2:4e581e5b39e8 291 angle3 = encoderC.getPulses() *PULSE_TO_RAD + angle3_init;
sabazerefa 2:4e581e5b39e8 292 velocity3 = encoderC.getVelocity() * PULSE_TO_RAD;
sabazerefa 2:4e581e5b39e8 293
sabazerefa 2:4e581e5b39e8 294 angle4 = encoderD.getPulses() * PULSE_TO_RAD + angle4_init;
sabazerefa 2:4e581e5b39e8 295 velocity4 = encoderD.getVelocity() * PULSE_TO_RAD;
sabazerefa 2:4e581e5b39e8 296
sabazerefa 1:25284247a74c 297
sabazerefa 1:25284247a74c 298 const float th1 = angle1;
sabazerefa 1:25284247a74c 299 const float th2 = angle2;
sabazerefa 1:25284247a74c 300 const float dth1= velocity1;
sabazerefa 1:25284247a74c 301 const float dth2= velocity2;
sabazerefa 1:25284247a74c 302
sabazerefa 2:4e581e5b39e8 303
sabazerefa 2:4e581e5b39e8 304 const float th3 = angle3;
sabazerefa 2:4e581e5b39e8 305 const float th4 = angle4;
sabazerefa 2:4e581e5b39e8 306 const float dth3= velocity3;
sabazerefa 2:4e581e5b39e8 307 const float dth4= velocity4;
sabazerefa 2:4e581e5b39e8 308
sabazerefa 2:4e581e5b39e8 309
sabazerefa 2:4e581e5b39e8 310
sabazerefa 1:25284247a74c 311 // Calculate the Jacobian
sabazerefa 1:25284247a74c 312 float Jx_th1 = l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1);
sabazerefa 1:25284247a74c 313 float Jx_th2 = l_AC*cos(th1+th2);
sabazerefa 1:25284247a74c 314 float Jy_th1 = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
sabazerefa 1:25284247a74c 315 float Jy_th2 = l_AC*sin(th1+th2);
sabazerefa 2:4e581e5b39e8 316
sabazerefa 2:4e581e5b39e8 317
sabazerefa 2:4e581e5b39e8 318 float Jx_th3 = l_AC*cos(th3+th4)+l_DE*cos(th3)+l_OB*cos(th3);
sabazerefa 2:4e581e5b39e8 319 float Jx_th4 = l_AC*cos(th3+th4);
sabazerefa 2:4e581e5b39e8 320 float Jy_th3 = l_AC*sin(th3+th4)+l_DE*sin(th3)+l_OB*sin(th3);
sabazerefa 2:4e581e5b39e8 321 float Jy_th4 = l_AC*sin(th3+th4);
sabazerefa 1:25284247a74c 322
sabazerefa 1:25284247a74c 323
sabazerefa 1:25284247a74c 324 // Calculate the forward kinematics (position and velocity)
sabazerefa 1:25284247a74c 325 float xFoot = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
sabazerefa 1:25284247a74c 326 float yFoot = -l_AC*cos(th1+th2)-l_DE*cos(th1)-l_OB*cos(th1);
sabazerefa 1:25284247a74c 327 float dxFoot = Jx_th1*dth1+Jx_th2*dth2;
sabazerefa 2:4e581e5b39e8 328 float dyFoot = Jy_th1*dth1+Jy_th2*dth2;
sabazerefa 2:4e581e5b39e8 329
sabazerefa 2:4e581e5b39e8 330
sabazerefa 2:4e581e5b39e8 331 float xFoot2 = l_AC*sin(th3+th4)+l_DE*sin(th3)+l_OB*sin(th3);
sabazerefa 2:4e581e5b39e8 332 float yFoot2 = -l_AC*cos(th3+th4)-l_DE*cos(th3)-l_OB*cos(th3);
sabazerefa 2:4e581e5b39e8 333 float dxFoot2 = Jx_th3*dth3+Jx_th4*dth4;
sabazerefa 2:4e581e5b39e8 334 float dyFoot2 = Jy_th3*dth3+Jy_th4*dth4;
sabazerefa 2:4e581e5b39e8 335
sabazerefa 1:25284247a74c 336
sabazerefa 1:25284247a74c 337 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
sabazerefa 1:25284247a74c 338 float teff = 0;
sabazerefa 1:25284247a74c 339 float vMult = 0;
sabazerefa 1:25284247a74c 340 if( t < start_period) {
sabazerefa 2:4e581e5b39e8 341 if (K_xx > 0 || K_yy > 0 || K_xx2 > 0 || K_yy2 >0) {
sabazerefa 1:25284247a74c 342 K_xx = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
sabazerefa 1:25284247a74c 343 K_yy = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
sabazerefa 1:25284247a74c 344 D_xx = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
sabazerefa 1:25284247a74c 345 D_yy = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
sabazerefa 1:25284247a74c 346 K_xy = 0;
sabazerefa 1:25284247a74c 347 D_xy = 0;
sabazerefa 2:4e581e5b39e8 348
sabazerefa 2:4e581e5b39e8 349 K_xx2=1;
sabazerefa 2:4e581e5b39e8 350 K_yy2=1;
sabazerefa 2:4e581e5b39e8 351 D_xx2=0.1;
sabazerefa 2:4e581e5b39e8 352 D_yy2=0.1;
sabazerefa 2:4e581e5b39e8 353 D_xy2=0;
sabazerefa 2:4e581e5b39e8 354 K_xy2=0;
sabazerefa 2:4e581e5b39e8 355
sabazerefa 2:4e581e5b39e8 356
sabazerefa 1:25284247a74c 357 }
sabazerefa 1:25284247a74c 358 teff = 0;
sabazerefa 1:25284247a74c 359 }
sabazerefa 1:25284247a74c 360 else if (t < start_period + traj_period)
sabazerefa 1:25284247a74c 361 {
sabazerefa 1:25284247a74c 362 K_xx = input_params[5]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 363 K_yy = input_params[6]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 364 K_xy = input_params[7]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 365 D_xx = input_params[8]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 366 D_yy = input_params[9]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 367 D_xy = input_params[10]; // Foot damping N/(m/s)
sabazerefa 2:4e581e5b39e8 368
sabazerefa 2:4e581e5b39e8 369 K_xx2 = input_params[14];
sabazerefa 2:4e581e5b39e8 370 K_yy2 = input_params[15];
sabazerefa 2:4e581e5b39e8 371 K_xy2 = input_params[16];
sabazerefa 2:4e581e5b39e8 372 D_xx2 = input_params[17];
sabazerefa 2:4e581e5b39e8 373 D_yy2 = input_params[18];
sabazerefa 2:4e581e5b39e8 374 D_xy2 = input_params[19];
sabazerefa 2:4e581e5b39e8 375
sabazerefa 2:4e581e5b39e8 376
sabazerefa 1:25284247a74c 377 teff = (t-start_period);
sabazerefa 1:25284247a74c 378 vMult = 1;
sabazerefa 1:25284247a74c 379 }
sabazerefa 1:25284247a74c 380 else
sabazerefa 1:25284247a74c 381 {
sabazerefa 1:25284247a74c 382 teff = traj_period;
sabazerefa 1:25284247a74c 383 vMult = 0;
sabazerefa 1:25284247a74c 384 }
sabazerefa 1:25284247a74c 385
sabazerefa 1:25284247a74c 386 // Get desired foot positions and velocities
sabazerefa 1:25284247a74c 387 float rDesFoot[2] , vDesFoot[2];
sabazerefa 1:25284247a74c 388 rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
sabazerefa 1:25284247a74c 389 rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
sabazerefa 1:25284247a74c 390 vDesFoot[0]/=traj_period;
sabazerefa 1:25284247a74c 391 vDesFoot[1]/=traj_period;
sabazerefa 1:25284247a74c 392 vDesFoot[0]*=vMult;
sabazerefa 1:25284247a74c 393 vDesFoot[1]*=vMult;
sabazerefa 1:25284247a74c 394
sabazerefa 2:4e581e5b39e8 395 float rDesFoot2[2] , vDesFoot2[2];
sabazerefa 2:4e581e5b39e8 396 float evalPoint = teff/traj_period+traj_period/2;
sabazerefa 2:4e581e5b39e8 397 if(evalPoint>traj_period) evalPoint=evalPoint-traj_period;
sabazerefa 3:2730130aa049 398 rDesFoot_bez.evaluate(evalPoint,rDesFoot2);
sabazerefa 2:4e581e5b39e8 399 rDesFoot_bez.evaluateDerivative(evalPoint,vDesFoot2);
sabazerefa 2:4e581e5b39e8 400 vDesFoot2[0]/=traj_period;
sabazerefa 2:4e581e5b39e8 401 vDesFoot2[1]/=traj_period;
sabazerefa 2:4e581e5b39e8 402 vDesFoot2[0]*=vMult;
sabazerefa 2:4e581e5b39e8 403 vDesFoot2[1]*=vMult;
sabazerefa 2:4e581e5b39e8 404
sabazerefa 1:25284247a74c 405 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
sabazerefa 1:25284247a74c 406 float xFoot_inv = -rDesFoot[0];
sabazerefa 1:25284247a74c 407 float yFoot_inv = rDesFoot[1];
sabazerefa 1:25284247a74c 408 float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
sabazerefa 1:25284247a74c 409 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)) ));
sabazerefa 1:25284247a74c 410 float th2_des = -(3.14159f - alpha);
sabazerefa 1:25284247a74c 411 float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
sabazerefa 1:25284247a74c 412
sabazerefa 2:4e581e5b39e8 413
sabazerefa 2:4e581e5b39e8 414 float xFoot_inv2 = -rDesFoot2[0];
sabazerefa 2:4e581e5b39e8 415 float yFoot_inv2 = rDesFoot2[1];
sabazerefa 2:4e581e5b39e8 416 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)) ));
sabazerefa 2:4e581e5b39e8 417 float th3_des = -(3.14159f - alpha2);
sabazerefa 2:4e581e5b39e8 418 float th4_des = -((3.14159f/2.0f) + atan2(yFoot_inv2,xFoot_inv2) - abs(asin( (l_AC/l_OE)*sin(alpha2) )));
sabazerefa 2:4e581e5b39e8 419
sabazerefa 2:4e581e5b39e8 420
sabazerefa 2:4e581e5b39e8 421
sabazerefa 1:25284247a74c 422 float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
sabazerefa 1:25284247a74c 423 float dth1_des = (1.0f/dd) * ( Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
sabazerefa 1:25284247a74c 424 float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
sabazerefa 2:4e581e5b39e8 425
sabazerefa 2:4e581e5b39e8 426 float dd2 = (Jx_th3*Jy_th4 - Jx_th4*Jy_th3);
sabazerefa 2:4e581e5b39e8 427 float dth3_des = (1.0f/dd2) * ( Jy_th4*vDesFoot[0] - Jx_th4*vDesFoot[1] );
sabazerefa 2:4e581e5b39e8 428 float dth4_des = (1.0f/dd2) * ( -Jy_th3*vDesFoot[0] + Jx_th3*vDesFoot[1] );
sabazerefa 1:25284247a74c 429
sabazerefa 1:25284247a74c 430 // Calculate error variables
sabazerefa 1:25284247a74c 431 float e_x = 0;
sabazerefa 1:25284247a74c 432 float e_y = 0;
sabazerefa 1:25284247a74c 433 float de_x = 0;
sabazerefa 1:25284247a74c 434 float de_y = 0;
sabazerefa 2:4e581e5b39e8 435
sabazerefa 2:4e581e5b39e8 436 float e_x2 = 0;
sabazerefa 2:4e581e5b39e8 437 float e_y2 = 0;
sabazerefa 2:4e581e5b39e8 438 float de_x2 = 0;
sabazerefa 2:4e581e5b39e8 439 float de_y2 = 0;
sabazerefa 1:25284247a74c 440
sabazerefa 1:25284247a74c 441 // Calculate virtual force on foot
sabazerefa 1:25284247a74c 442 float fx = K_xx*(rDesFoot[0]-xFoot) +K_xy*(rDesFoot[1]-yFoot)+D_xx*(vDesFoot[0]-dxFoot)+D_xy*(vDesFoot[1]-dyFoot);
sabazerefa 1:25284247a74c 443 float fy = K_xy*(rDesFoot[0]-xFoot) + K_yy*(rDesFoot[1]-yFoot) + D_xy*(vDesFoot[0]-xFoot)+D_yy*(vDesFoot[1]-dyFoot);
sabazerefa 2:4e581e5b39e8 444
sabazerefa 2:4e581e5b39e8 445 float fx2 = K_xx2*(rDesFoot2[0]-xFoot2) +K_xy*(rDesFoot2[1]-yFoot2)+D_xx2*(vDesFoot2[0]-dxFoot2)+D_xy2*(vDesFoot2[1]-dyFoot2);
sabazerefa 2:4e581e5b39e8 446 float fy2 = K_xy2*(rDesFoot2[0]-xFoot2) + K_yy*(rDesFoot2[1]-yFoot2) + D_xy2*(vDesFoot2[0]-xFoot2)+D_yy2*(vDesFoot2[1]-dyFoot2);
sabazerefa 2:4e581e5b39e8 447
sabazerefa 2:4e581e5b39e8 448
sabazerefa 1:25284247a74c 449 // Set desired currents
sabazerefa 1:25284247a74c 450 current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;
sabazerefa 1:25284247a74c 451 current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;
sabazerefa 1:25284247a74c 452 current_des3 = (Jx_th1*fx+Jy_th1*fy)/k_t;
sabazerefa 1:25284247a74c 453 current_des4 = (Jx_th2*fx+Jy_th2*fy)/k_t;
sabazerefa 1:25284247a74c 454
sabazerefa 1:25284247a74c 455
sabazerefa 1:25284247a74c 456
sabazerefa 1:25284247a74c 457 // Joint impedance
sabazerefa 1:25284247a74c 458 // sub Kxx for K1, Dxx for D1, Kyy for K2, Dyy for D2
sabazerefa 1:25284247a74c 459 // Note: Be careful with signs now that you have non-zero desired angles!
sabazerefa 1:25284247a74c 460 // Your equations should be of the form i_d = K1*(q1_d - q1) + D1*(dq1_d - dq1)
sabazerefa 1:25284247a74c 461
sabazerefa 1:25284247a74c 462 //PART 0A FIRST
sabazerefa 1:25284247a74c 463 // float q1_d=0;
sabazerefa 1:25284247a74c 464 // float dq1_d=0;
sabazerefa 1:25284247a74c 465 // current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;
sabazerefa 1:25284247a74c 466 // current_des2 = 0;
sabazerefa 1:25284247a74c 467
sabazerefa 1:25284247a74c 468 // PART 2
sabazerefa 1:25284247a74c 469 // float q1_d=th1_des;
sabazerefa 1:25284247a74c 470 // float dq1_d=dth1_des;
sabazerefa 1:25284247a74c 471 // float q2_d=th2_des;
sabazerefa 1:25284247a74c 472 // float dq2_d=dth2_des;
sabazerefa 1:25284247a74c 473 // current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;
sabazerefa 1:25284247a74c 474 // current_des2 = (K_yy*(q2_d-th2) + D_yy*(dq2_d-dth2))/k_t;
sabazerefa 1:25284247a74c 475
sabazerefa 1:25284247a74c 476
sabazerefa 1:25284247a74c 477
sabazerefa 1:25284247a74c 478 /* PART 3!!!!!!!!!!!!!!!!*/
sabazerefa 1:25284247a74c 479
sabazerefa 1:25284247a74c 480 // Cartesian impedance
sabazerefa 1:25284247a74c 481 // Note: As with the joint space laws, be careful with signs!
sabazerefa 1:25284247a74c 482 // current_des1 = 0;
sabazerefa 1:25284247a74c 483 // current_des2 = 0;
sabazerefa 1:25284247a74c 484
sabazerefa 1:25284247a74c 485
sabazerefa 1:25284247a74c 486 // Form output to send to MATLAB
sabazerefa 1:25284247a74c 487 float output_data[NUM_OUTPUTS];
sabazerefa 1:25284247a74c 488 // current time
sabazerefa 1:25284247a74c 489 output_data[0] = t.read();
sabazerefa 1:25284247a74c 490 // motor 1 state
sabazerefa 1:25284247a74c 491 output_data[1] = angle1;
sabazerefa 1:25284247a74c 492 output_data[2] = velocity1;
sabazerefa 1:25284247a74c 493 output_data[3] = current1;
sabazerefa 1:25284247a74c 494 output_data[4] = current_des1;
sabazerefa 1:25284247a74c 495 output_data[5] = duty_cycle1;
sabazerefa 1:25284247a74c 496 // motor 2 state
sabazerefa 1:25284247a74c 497 output_data[6] = angle2;
sabazerefa 1:25284247a74c 498 output_data[7] = velocity2;
sabazerefa 1:25284247a74c 499 output_data[8] = current2;
sabazerefa 1:25284247a74c 500 output_data[9] = current_des2;
sabazerefa 1:25284247a74c 501 output_data[10]= duty_cycle2;
sabazerefa 2:4e581e5b39e8 502 // motor 3 state
sabazerefa 2:4e581e5b39e8 503 output_data[11] = angle2;
sabazerefa 3:2730130aa049 504 output_data[12] = velocity2;
sabazerefa 2:4e581e5b39e8 505 output_data[13] = current2;
sabazerefa 2:4e581e5b39e8 506 output_data[14] = current_des2;
sabazerefa 2:4e581e5b39e8 507 output_data[15]= duty_cycle2;
sabazerefa 2:4e581e5b39e8 508 // motor 4 state
sabazerefa 2:4e581e5b39e8 509 output_data[16] = angle2;
sabazerefa 3:2730130aa049 510 output_data[17] = velocity2;
sabazerefa 2:4e581e5b39e8 511 output_data[18] = current2;
sabazerefa 2:4e581e5b39e8 512 output_data[19] = current_des2;
sabazerefa 2:4e581e5b39e8 513 output_data[20]= duty_cycle2;
sabazerefa 1:25284247a74c 514 // foot state
sabazerefa 2:4e581e5b39e8 515 output_data[21] = xFoot;
sabazerefa 2:4e581e5b39e8 516 output_data[22] = yFoot;
sabazerefa 2:4e581e5b39e8 517 output_data[23] = dxFoot;
sabazerefa 2:4e581e5b39e8 518 output_data[24] = dyFoot;
sabazerefa 2:4e581e5b39e8 519 output_data[25] = rDesFoot[0];
sabazerefa 2:4e581e5b39e8 520 output_data[26] = rDesFoot[1];
sabazerefa 2:4e581e5b39e8 521 output_data[27] = vDesFoot[0];
sabazerefa 2:4e581e5b39e8 522 output_data[28] = vDesFoot[1];
sabazerefa 1:25284247a74c 523
sabazerefa 1:25284247a74c 524 // Send data to MATLAB
sabazerefa 1:25284247a74c 525 server.sendData(output_data,NUM_OUTPUTS);
sabazerefa 1:25284247a74c 526
sabazerefa 1:25284247a74c 527 wait_us(impedance_control_period_us);
sabazerefa 1:25284247a74c 528 }
sabazerefa 1:25284247a74c 529
sabazerefa 1:25284247a74c 530 // Cleanup after experiment
sabazerefa 1:25284247a74c 531 server.setExperimentComplete();
sabazerefa 1:25284247a74c 532 currentLoop.detach();
sabazerefa 1:25284247a74c 533 motorShield.motorAWrite(0, 0); //turn motor A off
sabazerefa 1:25284247a74c 534 motorShield.motorBWrite(0, 0); //turn motor B off
sabazerefa 1:25284247a74c 535 motorShield.motorCWrite(0,0);
sabazerefa 1:25284247a74c 536 motorShield.motorDWrite(0,0);
sabazerefa 1:25284247a74c 537
sabazerefa 1:25284247a74c 538 } // end if
sabazerefa 1:25284247a74c 539
sabazerefa 1:25284247a74c 540 } // end while
sabazerefa 1:25284247a74c 541
sabazerefa 1:25284247a74c 542 } // end main
sabazerefa 1:25284247a74c 543