Sebastian Uribe / Mbed OS pan_flipping

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MatrixMath Matrix ExperimentServer QEI_pmw MotorShield

main.cpp@35:88dbfefc1bbb, 2020-11-16 (annotated)

Committer:
dgdiaz
Date:
Mon Nov 16 22:10:05 2020 +0000
Revision:
35:88dbfefc1bbb
Parent:
34:9a24d0f718ac
Child:
36:a3beb771d8f7
dcdcsa

Who changed what in which revision?

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