Saba Zerefa / Mbed OS 274TeamProject

2.74 team project

Dependencies:   ExperimentServer QEI_pmw MotorShield

main.cpp@1:25284247a74c, 2021-11-22 (annotated)

Committer:
sabazerefa
Date:
Mon Nov 22 07:41:36 2021 +0000
Revision:
1:25284247a74c
Child:
2:4e581e5b39e8
Implemented two motors, only copying desired. not input interface w matlab

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 1:25284247a74c 13 #define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
sabazerefa 1:25284247a74c 14 #define NUM_OUTPUTS 19
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 1:25284247a74c 28
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 1:25284247a74c 52 // Fixed kinematic parameters
sabazerefa 1:25284247a74c 53 const float l_OA=.011;
sabazerefa 1:25284247a74c 54 const float l_OB=.042;
sabazerefa 1:25284247a74c 55 const float l_AC=.096;
sabazerefa 1:25284247a74c 56 const float l_DE=.091;
sabazerefa 1:25284247a74c 57
sabazerefa 1:25284247a74c 58 // Timing parameters
sabazerefa 1:25284247a74c 59 float current_control_period_us = 200.0f; // 5kHz current control loop
sabazerefa 1:25284247a74c 60 float impedance_control_period_us = 1000.0f; // 1kHz impedance control loop
sabazerefa 1:25284247a74c 61 float start_period, traj_period, end_period;
sabazerefa 1:25284247a74c 62
sabazerefa 1:25284247a74c 63 // Control parameters
sabazerefa 1:25284247a74c 64 float current_Kp = 4.0f;
sabazerefa 1:25284247a74c 65 float current_Ki = 0.4f;
sabazerefa 1:25284247a74c 66 float current_int_max = 3.0f;
sabazerefa 1:25284247a74c 67 float duty_max;
sabazerefa 1:25284247a74c 68 float K_xx;
sabazerefa 1:25284247a74c 69 float K_yy;
sabazerefa 1:25284247a74c 70 float K_xy;
sabazerefa 1:25284247a74c 71 float D_xx;
sabazerefa 1:25284247a74c 72 float D_xy;
sabazerefa 1:25284247a74c 73 float D_yy;
sabazerefa 1:25284247a74c 74
sabazerefa 1:25284247a74c 75 // Model parameters
sabazerefa 1:25284247a74c 76 float supply_voltage = 12; // motor supply voltage
sabazerefa 1:25284247a74c 77 float R = 2.0f; // motor resistance
sabazerefa 1:25284247a74c 78 float k_t = 0.18f; // motor torque constant
sabazerefa 1:25284247a74c 79 float nu = 0.0005; // motor viscous friction
sabazerefa 1:25284247a74c 80
sabazerefa 1:25284247a74c 81 // Current control interrupt function
sabazerefa 1:25284247a74c 82 void CurrentLoop()
sabazerefa 1:25284247a74c 83 {
sabazerefa 1:25284247a74c 84 // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
sabazerefa 1:25284247a74c 85
sabazerefa 1:25284247a74c 86 //use the motor shield as follows:
sabazerefa 1:25284247a74c 87 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
sabazerefa 1:25284247a74c 88
sabazerefa 1:25284247a74c 89 current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 90 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 91 float err_c1 = current_des1 - current1; // current errror
sabazerefa 1:25284247a74c 92 current_int1 += err_c1; // integrate error
sabazerefa 1:25284247a74c 93 current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 94 float ff1 = R*current_des1 + k_t*velocity1; // feedforward terms
sabazerefa 1:25284247a74c 95 duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 96
sabazerefa 1:25284247a74c 97 float absDuty1 = abs(duty_cycle1);
sabazerefa 1:25284247a74c 98 if (absDuty1 > duty_max) {
sabazerefa 1:25284247a74c 99 duty_cycle1 *= duty_max / absDuty1;
sabazerefa 1:25284247a74c 100 absDuty1 = duty_max;
sabazerefa 1:25284247a74c 101 }
sabazerefa 1:25284247a74c 102 if (duty_cycle1 < 0) { // backwards
sabazerefa 1:25284247a74c 103 motorShield.motorAWrite(absDuty1, 1);
sabazerefa 1:25284247a74c 104 } else { // forwards
sabazerefa 1:25284247a74c 105 motorShield.motorAWrite(absDuty1, 0);
sabazerefa 1:25284247a74c 106 }
sabazerefa 1:25284247a74c 107 prev_current_des1 = current_des1;
sabazerefa 1:25284247a74c 108
sabazerefa 1:25284247a74c 109 current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 110 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 111 float err_c2 = current_des2 - current2; // current error
sabazerefa 1:25284247a74c 112 current_int2 += err_c2; // integrate error
sabazerefa 1:25284247a74c 113 current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 114 float ff2 = R*current_des2 + k_t*velocity2; // feedforward terms
sabazerefa 1:25284247a74c 115 duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 116
sabazerefa 1:25284247a74c 117 float absDuty2 = abs(duty_cycle2);
sabazerefa 1:25284247a74c 118 if (absDuty2 > duty_max) {
sabazerefa 1:25284247a74c 119 duty_cycle2 *= duty_max / absDuty2;
sabazerefa 1:25284247a74c 120 absDuty2 = duty_max;
sabazerefa 1:25284247a74c 121 }
sabazerefa 1:25284247a74c 122 if (duty_cycle2 < 0) { // backwards
sabazerefa 1:25284247a74c 123 motorShield.motorBWrite(absDuty2, 1);
sabazerefa 1:25284247a74c 124 } else { // forwards
sabazerefa 1:25284247a74c 125 motorShield.motorBWrite(absDuty2, 0);
sabazerefa 1:25284247a74c 126 }
sabazerefa 1:25284247a74c 127 prev_current_des2 = current_des2;
sabazerefa 1:25284247a74c 128
sabazerefa 1:25284247a74c 129
sabazerefa 1:25284247a74c 130 current3 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 131 velocity3 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 132 float err_c3 = current_des3 - current3; // current errror
sabazerefa 1:25284247a74c 133 current_int3 += err_c3; // integrate error
sabazerefa 1:25284247a74c 134 current_int3 = fmaxf( fminf(current_int3, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 135 float ff3 = R*current_des3 + k_t*velocity3; // feedforward terms
sabazerefa 1:25284247a74c 136 duty_cycle3 = (ff3 + current_Kp*err_c3 + current_Ki*current_int3)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 137
sabazerefa 1:25284247a74c 138 float absDuty3 = abs(duty_cycle3);
sabazerefa 1:25284247a74c 139 if (absDuty3 > duty_max) {
sabazerefa 1:25284247a74c 140 duty_cycle3 *= duty_max / absDuty3;
sabazerefa 1:25284247a74c 141 absDuty3 = duty_max;
sabazerefa 1:25284247a74c 142 }
sabazerefa 1:25284247a74c 143 if (duty_cycle3 < 0) { // backwards
sabazerefa 1:25284247a74c 144 motorShield.motorCWrite(absDuty3, 1);
sabazerefa 1:25284247a74c 145 } else { // forwards
sabazerefa 1:25284247a74c 146 motorShield.motorCWrite(absDuty3, 0);
sabazerefa 1:25284247a74c 147 }
sabazerefa 1:25284247a74c 148 prev_current_des3 = current_des3;
sabazerefa 1:25284247a74c 149
sabazerefa 1:25284247a74c 150
sabazerefa 1:25284247a74c 151 current4 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f); // measure current
sabazerefa 1:25284247a74c 152 velocity4 = encoderA.getVelocity() * PULSE_TO_RAD; // measure velocity
sabazerefa 1:25284247a74c 153 float err_c4 = current_des4 - current4; // current errror
sabazerefa 1:25284247a74c 154 current_int4 += err_c4; // integrate error
sabazerefa 1:25284247a74c 155 current_int4 = fmaxf( fminf(current_int4, current_int_max), -current_int_max); // anti-windup
sabazerefa 1:25284247a74c 156 float ff4 = R*current_des4 + k_t*velocity4; // feedforward terms
sabazerefa 1:25284247a74c 157 duty_cycle4 = (ff4 + current_Kp*err_c4 + current_Ki*current_int4)/supply_voltage; // PI current controller
sabazerefa 1:25284247a74c 158
sabazerefa 1:25284247a74c 159 float absDuty4 = abs(duty_cycle4);
sabazerefa 1:25284247a74c 160 if (absDuty4 > duty_max) {
sabazerefa 1:25284247a74c 161 duty_cycle4 *= duty_max / absDuty4;
sabazerefa 1:25284247a74c 162 absDuty4 = duty_max;
sabazerefa 1:25284247a74c 163 }
sabazerefa 1:25284247a74c 164 if (duty_cycle4 < 0) { // backwards
sabazerefa 1:25284247a74c 165 motorShield.motorCWrite(absDuty4, 1);
sabazerefa 1:25284247a74c 166 } else { // forwards
sabazerefa 1:25284247a74c 167 motorShield.motorCWrite(absDuty4, 0);
sabazerefa 1:25284247a74c 168 }
sabazerefa 1:25284247a74c 169 prev_current_des4 = current_des4;
sabazerefa 1:25284247a74c 170
sabazerefa 1:25284247a74c 171
sabazerefa 1:25284247a74c 172 }
sabazerefa 1:25284247a74c 173
sabazerefa 1:25284247a74c 174 int main (void)
sabazerefa 1:25284247a74c 175 {
sabazerefa 1:25284247a74c 176
sabazerefa 1:25284247a74c 177 // Object for 7th order Cartesian foot trajectory.
sabazerefa 1:25284247a74c 178
sabazerefa 1:25284247a74c 179 //CREATE A TRAJECTORY
sabazerefa 1:25284247a74c 180 BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
sabazerefa 1:25284247a74c 181
sabazerefa 1:25284247a74c 182 // Link the terminal with our server and start it up
sabazerefa 1:25284247a74c 183 server.attachTerminal(pc);
sabazerefa 1:25284247a74c 184 server.init();
sabazerefa 1:25284247a74c 185
sabazerefa 1:25284247a74c 186 // Continually get input from MATLAB and run experiments
sabazerefa 1:25284247a74c 187 float input_params[NUM_INPUTS];
sabazerefa 1:25284247a74c 188 pc.printf("%f",input_params[0]);
sabazerefa 1:25284247a74c 189
sabazerefa 1:25284247a74c 190 while(1) {
sabazerefa 1:25284247a74c 191
sabazerefa 1:25284247a74c 192 // If there are new inputs, this code will run
sabazerefa 1:25284247a74c 193 if (server.getParams(input_params,NUM_INPUTS)) {
sabazerefa 1:25284247a74c 194
sabazerefa 1:25284247a74c 195
sabazerefa 1:25284247a74c 196 // Get inputs from MATLAB
sabazerefa 1:25284247a74c 197 start_period = input_params[0]; // First buffer time, before trajectory
sabazerefa 1:25284247a74c 198 traj_period = input_params[1]; // Trajectory time/length
sabazerefa 1:25284247a74c 199 end_period = input_params[2]; // Second buffer time, after trajectory
sabazerefa 1:25284247a74c 200
sabazerefa 1:25284247a74c 201 angle1_init = input_params[3]; // Initial angle for q1 (rad)
sabazerefa 1:25284247a74c 202 angle2_init = input_params[4]; // Initial angle for q2 (rad)
sabazerefa 1:25284247a74c 203
sabazerefa 1:25284247a74c 204 K_xx = input_params[5]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 205 K_yy = input_params[6]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 206 K_xy = input_params[7]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 207 D_xx = input_params[8]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 208 D_yy = input_params[9]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 209 D_xy = input_params[10]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 210 duty_max = input_params[11]; // Maximum duty factor
sabazerefa 1:25284247a74c 211
sabazerefa 1:25284247a74c 212 // Get foot trajectory points
sabazerefa 1:25284247a74c 213 float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
sabazerefa 1:25284247a74c 214 for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
sabazerefa 1:25284247a74c 215 foot_pts[i] = input_params[12+i];
sabazerefa 1:25284247a74c 216 }
sabazerefa 1:25284247a74c 217 rDesFoot_bez.setPoints(foot_pts);
sabazerefa 1:25284247a74c 218
sabazerefa 1:25284247a74c 219 // Attach current loop interrupt
sabazerefa 1:25284247a74c 220 currentLoop.attach_us(CurrentLoop,current_control_period_us);
sabazerefa 1:25284247a74c 221
sabazerefa 1:25284247a74c 222 // Setup experiment
sabazerefa 1:25284247a74c 223 t.reset();
sabazerefa 1:25284247a74c 224 t.start();
sabazerefa 1:25284247a74c 225 encoderA.reset();
sabazerefa 1:25284247a74c 226 encoderB.reset();
sabazerefa 1:25284247a74c 227 encoderC.reset();
sabazerefa 1:25284247a74c 228 encoderD.reset();
sabazerefa 1:25284247a74c 229
sabazerefa 1:25284247a74c 230 motorShield.motorAWrite(0, 0); //turn motor A off
sabazerefa 1:25284247a74c 231 motorShield.motorBWrite(0, 0); //turn motor B off
sabazerefa 1:25284247a74c 232
sabazerefa 1:25284247a74c 233 // Run experiment
sabazerefa 1:25284247a74c 234 while( t.read() < start_period + traj_period + end_period) {
sabazerefa 1:25284247a74c 235
sabazerefa 1:25284247a74c 236 // Read encoders to get motor states
sabazerefa 1:25284247a74c 237 angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
sabazerefa 1:25284247a74c 238 velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
sabazerefa 1:25284247a74c 239
sabazerefa 1:25284247a74c 240 angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;
sabazerefa 1:25284247a74c 241 velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
sabazerefa 1:25284247a74c 242
sabazerefa 1:25284247a74c 243 const float th1 = angle1;
sabazerefa 1:25284247a74c 244 const float th2 = angle2;
sabazerefa 1:25284247a74c 245 const float dth1= velocity1;
sabazerefa 1:25284247a74c 246 const float dth2= velocity2;
sabazerefa 1:25284247a74c 247
sabazerefa 1:25284247a74c 248 // Calculate the Jacobian
sabazerefa 1:25284247a74c 249 float Jx_th1 = l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1);
sabazerefa 1:25284247a74c 250 float Jx_th2 = l_AC*cos(th1+th2);
sabazerefa 1:25284247a74c 251 float Jy_th1 = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
sabazerefa 1:25284247a74c 252 float Jy_th2 = l_AC*sin(th1+th2);
sabazerefa 1:25284247a74c 253
sabazerefa 1:25284247a74c 254
sabazerefa 1:25284247a74c 255 // Calculate the forward kinematics (position and velocity)
sabazerefa 1:25284247a74c 256 float xFoot = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
sabazerefa 1:25284247a74c 257 float yFoot = -l_AC*cos(th1+th2)-l_DE*cos(th1)-l_OB*cos(th1);
sabazerefa 1:25284247a74c 258 float dxFoot = Jx_th1*dth1+Jx_th2*dth2;
sabazerefa 1:25284247a74c 259 float dyFoot = Jy_th1*dth1+Jy_th2*dth2;
sabazerefa 1:25284247a74c 260
sabazerefa 1:25284247a74c 261 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
sabazerefa 1:25284247a74c 262 float teff = 0;
sabazerefa 1:25284247a74c 263 float vMult = 0;
sabazerefa 1:25284247a74c 264 if( t < start_period) {
sabazerefa 1:25284247a74c 265 if (K_xx > 0 || K_yy > 0) {
sabazerefa 1:25284247a74c 266 K_xx = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
sabazerefa 1:25284247a74c 267 K_yy = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
sabazerefa 1:25284247a74c 268 D_xx = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
sabazerefa 1:25284247a74c 269 D_yy = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
sabazerefa 1:25284247a74c 270 K_xy = 0;
sabazerefa 1:25284247a74c 271 D_xy = 0;
sabazerefa 1:25284247a74c 272 }
sabazerefa 1:25284247a74c 273 teff = 0;
sabazerefa 1:25284247a74c 274 }
sabazerefa 1:25284247a74c 275 else if (t < start_period + traj_period)
sabazerefa 1:25284247a74c 276 {
sabazerefa 1:25284247a74c 277 K_xx = input_params[5]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 278 K_yy = input_params[6]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 279 K_xy = input_params[7]; // Foot stiffness N/m
sabazerefa 1:25284247a74c 280 D_xx = input_params[8]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 281 D_yy = input_params[9]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 282 D_xy = input_params[10]; // Foot damping N/(m/s)
sabazerefa 1:25284247a74c 283 teff = (t-start_period);
sabazerefa 1:25284247a74c 284 vMult = 1;
sabazerefa 1:25284247a74c 285 }
sabazerefa 1:25284247a74c 286 else
sabazerefa 1:25284247a74c 287 {
sabazerefa 1:25284247a74c 288 teff = traj_period;
sabazerefa 1:25284247a74c 289 vMult = 0;
sabazerefa 1:25284247a74c 290 }
sabazerefa 1:25284247a74c 291
sabazerefa 1:25284247a74c 292 // Get desired foot positions and velocities
sabazerefa 1:25284247a74c 293 float rDesFoot[2] , vDesFoot[2];
sabazerefa 1:25284247a74c 294 rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
sabazerefa 1:25284247a74c 295 rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
sabazerefa 1:25284247a74c 296 vDesFoot[0]/=traj_period;
sabazerefa 1:25284247a74c 297 vDesFoot[1]/=traj_period;
sabazerefa 1:25284247a74c 298 vDesFoot[0]*=vMult;
sabazerefa 1:25284247a74c 299 vDesFoot[1]*=vMult;
sabazerefa 1:25284247a74c 300
sabazerefa 1:25284247a74c 301 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
sabazerefa 1:25284247a74c 302 float xFoot_inv = -rDesFoot[0];
sabazerefa 1:25284247a74c 303 float yFoot_inv = rDesFoot[1];
sabazerefa 1:25284247a74c 304 float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
sabazerefa 1:25284247a74c 305 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 306 float th2_des = -(3.14159f - alpha);
sabazerefa 1:25284247a74c 307 float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
sabazerefa 1:25284247a74c 308
sabazerefa 1:25284247a74c 309 float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
sabazerefa 1:25284247a74c 310 float dth1_des = (1.0f/dd) * ( Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
sabazerefa 1:25284247a74c 311 float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
sabazerefa 1:25284247a74c 312
sabazerefa 1:25284247a74c 313 // Calculate error variables
sabazerefa 1:25284247a74c 314 float e_x = 0;
sabazerefa 1:25284247a74c 315 float e_y = 0;
sabazerefa 1:25284247a74c 316 float de_x = 0;
sabazerefa 1:25284247a74c 317 float de_y = 0;
sabazerefa 1:25284247a74c 318
sabazerefa 1:25284247a74c 319 // Calculate virtual force on foot
sabazerefa 1:25284247a74c 320 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 321 float fy = K_xy*(rDesFoot[0]-xFoot) + K_yy*(rDesFoot[1]-yFoot) + D_xy*(vDesFoot[0]-xFoot)+D_yy*(vDesFoot[1]-dyFoot);
sabazerefa 1:25284247a74c 322
sabazerefa 1:25284247a74c 323 // Set desired currents
sabazerefa 1:25284247a74c 324 current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;
sabazerefa 1:25284247a74c 325 current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;
sabazerefa 1:25284247a74c 326 current_des3 = (Jx_th1*fx+Jy_th1*fy)/k_t;
sabazerefa 1:25284247a74c 327 current_des4 = (Jx_th2*fx+Jy_th2*fy)/k_t;
sabazerefa 1:25284247a74c 328
sabazerefa 1:25284247a74c 329
sabazerefa 1:25284247a74c 330
sabazerefa 1:25284247a74c 331 // Joint impedance
sabazerefa 1:25284247a74c 332 // sub Kxx for K1, Dxx for D1, Kyy for K2, Dyy for D2
sabazerefa 1:25284247a74c 333 // Note: Be careful with signs now that you have non-zero desired angles!
sabazerefa 1:25284247a74c 334 // Your equations should be of the form i_d = K1*(q1_d - q1) + D1*(dq1_d - dq1)
sabazerefa 1:25284247a74c 335
sabazerefa 1:25284247a74c 336 //PART 0A FIRST
sabazerefa 1:25284247a74c 337 // float q1_d=0;
sabazerefa 1:25284247a74c 338 // float dq1_d=0;
sabazerefa 1:25284247a74c 339 // current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;
sabazerefa 1:25284247a74c 340 // current_des2 = 0;
sabazerefa 1:25284247a74c 341
sabazerefa 1:25284247a74c 342 // PART 2
sabazerefa 1:25284247a74c 343 // float q1_d=th1_des;
sabazerefa 1:25284247a74c 344 // float dq1_d=dth1_des;
sabazerefa 1:25284247a74c 345 // float q2_d=th2_des;
sabazerefa 1:25284247a74c 346 // float dq2_d=dth2_des;
sabazerefa 1:25284247a74c 347 // current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;
sabazerefa 1:25284247a74c 348 // current_des2 = (K_yy*(q2_d-th2) + D_yy*(dq2_d-dth2))/k_t;
sabazerefa 1:25284247a74c 349
sabazerefa 1:25284247a74c 350
sabazerefa 1:25284247a74c 351
sabazerefa 1:25284247a74c 352 /* PART 3!!!!!!!!!!!!!!!!*/
sabazerefa 1:25284247a74c 353
sabazerefa 1:25284247a74c 354 // Cartesian impedance
sabazerefa 1:25284247a74c 355 // Note: As with the joint space laws, be careful with signs!
sabazerefa 1:25284247a74c 356 // current_des1 = 0;
sabazerefa 1:25284247a74c 357 // current_des2 = 0;
sabazerefa 1:25284247a74c 358
sabazerefa 1:25284247a74c 359
sabazerefa 1:25284247a74c 360 // Form output to send to MATLAB
sabazerefa 1:25284247a74c 361 float output_data[NUM_OUTPUTS];
sabazerefa 1:25284247a74c 362 // current time
sabazerefa 1:25284247a74c 363 output_data[0] = t.read();
sabazerefa 1:25284247a74c 364 // motor 1 state
sabazerefa 1:25284247a74c 365 output_data[1] = angle1;
sabazerefa 1:25284247a74c 366 output_data[2] = velocity1;
sabazerefa 1:25284247a74c 367 output_data[3] = current1;
sabazerefa 1:25284247a74c 368 output_data[4] = current_des1;
sabazerefa 1:25284247a74c 369 output_data[5] = duty_cycle1;
sabazerefa 1:25284247a74c 370 // motor 2 state
sabazerefa 1:25284247a74c 371 output_data[6] = angle2;
sabazerefa 1:25284247a74c 372 output_data[7] = velocity2;
sabazerefa 1:25284247a74c 373 output_data[8] = current2;
sabazerefa 1:25284247a74c 374 output_data[9] = current_des2;
sabazerefa 1:25284247a74c 375 output_data[10]= duty_cycle2;
sabazerefa 1:25284247a74c 376 // foot state
sabazerefa 1:25284247a74c 377 output_data[11] = xFoot;
sabazerefa 1:25284247a74c 378 output_data[12] = yFoot;
sabazerefa 1:25284247a74c 379 output_data[13] = dxFoot;
sabazerefa 1:25284247a74c 380 output_data[14] = dyFoot;
sabazerefa 1:25284247a74c 381 output_data[15] = rDesFoot[0];
sabazerefa 1:25284247a74c 382 output_data[16] = rDesFoot[1];
sabazerefa 1:25284247a74c 383 output_data[17] = vDesFoot[0];
sabazerefa 1:25284247a74c 384 output_data[18] = vDesFoot[1];
sabazerefa 1:25284247a74c 385
sabazerefa 1:25284247a74c 386 // Send data to MATLAB
sabazerefa 1:25284247a74c 387 server.sendData(output_data,NUM_OUTPUTS);
sabazerefa 1:25284247a74c 388
sabazerefa 1:25284247a74c 389 wait_us(impedance_control_period_us);
sabazerefa 1:25284247a74c 390 }
sabazerefa 1:25284247a74c 391
sabazerefa 1:25284247a74c 392 // Cleanup after experiment
sabazerefa 1:25284247a74c 393 server.setExperimentComplete();
sabazerefa 1:25284247a74c 394 currentLoop.detach();
sabazerefa 1:25284247a74c 395 motorShield.motorAWrite(0, 0); //turn motor A off
sabazerefa 1:25284247a74c 396 motorShield.motorBWrite(0, 0); //turn motor B off
sabazerefa 1:25284247a74c 397 motorShield.motorCWrite(0,0);
sabazerefa 1:25284247a74c 398 motorShield.motorDWrite(0,0);
sabazerefa 1:25284247a74c 399
sabazerefa 1:25284247a74c 400 } // end if
sabazerefa 1:25284247a74c 401
sabazerefa 1:25284247a74c 402 } // end while
sabazerefa 1:25284247a74c 403
sabazerefa 1:25284247a74c 404 } // end main
sabazerefa 1:25284247a74c 405