Emily Kamienski / Mbed OS swing_jump_win

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:   ExperimentServer MotorShield QEI_pmw

main.cpp@0:f7e7848048d5, 2022-11-28 (annotated)

Committer:
kaymies
Date:
Mon Nov 28 22:18:47 2022 +0000
Revision:
0:f7e7848048d5
Child:
1:c4fa1bfe7672
swing jump win code;

Who changed what in which revision?

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