2.131 test rig DAQ for MTB suspension characterization project
Dependencies: EthernetInterface ExperimentServer HX711 QEI_pmw mbed-rtos mbed
Fork of Dolphin by
main.cpp@4:300ced917633, 2015-11-25 (annotated)
- Committer:
- laskowsk
- Date:
- Wed Nov 25 20:24:31 2015 +0000
- Revision:
- 4:300ced917633
- Parent:
- 3:c8e53b5762bd
- Child:
- 6:b7f6433cc765
Committed
Who changed what in which revision?
User | Revision | Line number | New 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" |
laskowsk | 4:300ced917633 | 6 | #include "HX711.h" |
pwensing | 0:43448bf056e8 | 7 | |
laskowsk | 4:300ced917633 | 8 | #define NUM_INPUTS 21 |
laskowsk | 4:300ced917633 | 9 | #define NUM_OUTPUTS 17 |
laskowsk | 4:300ced917633 | 10 | #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f) |
pwensing | 0:43448bf056e8 | 11 | |
pwensing | 0:43448bf056e8 | 12 | Serial pc(USBTX, USBRX); // USB Serial Terminal |
pwensing | 0:43448bf056e8 | 13 | ExperimentServer server; // Object that lets us communicate with MATLAB |
pwensing | 0:43448bf056e8 | 14 | Timer t; // Timer to measure elapsed time of experiment |
laskowsk | 4:300ced917633 | 15 | |
laskowsk | 4:300ced917633 | 16 | // Variables for q1 |
laskowsk | 4:300ced917633 | 17 | float current1; |
laskowsk | 4:300ced917633 | 18 | float current_des1; |
laskowsk | 4:300ced917633 | 19 | float angle1; |
laskowsk | 4:300ced917633 | 20 | float angle_des1; |
laskowsk | 4:300ced917633 | 21 | float velocity1; |
laskowsk | 4:300ced917633 | 22 | float velocity_des1; |
laskowsk | 4:300ced917633 | 23 | float duty_factor1; |
laskowsk | 4:300ced917633 | 24 | float angle1_init; |
laskowsk | 4:300ced917633 | 25 | float max_angle1; |
laskowsk | 4:300ced917633 | 26 | float dif_ang1; |
laskowsk | 4:300ced917633 | 27 | |
laskowsk | 4:300ced917633 | 28 | // Variables for q2 |
laskowsk | 4:300ced917633 | 29 | float current2; |
laskowsk | 4:300ced917633 | 30 | float current_des2; |
laskowsk | 4:300ced917633 | 31 | float angle2; |
laskowsk | 4:300ced917633 | 32 | float angle_des2; |
laskowsk | 4:300ced917633 | 33 | float velocity2; |
laskowsk | 4:300ced917633 | 34 | float velocity_des2; |
laskowsk | 4:300ced917633 | 35 | float duty_factor2; |
laskowsk | 4:300ced917633 | 36 | float angle2_init; |
laskowsk | 4:300ced917633 | 37 | float max_angle2; |
laskowsk | 4:300ced917633 | 38 | float dif_ang2; |
laskowsk | 4:300ced917633 | 39 | |
laskowsk | 4:300ced917633 | 40 | // Fixed kinematic parameters |
laskowsk | 4:300ced917633 | 41 | const float l_OA=.010; |
laskowsk | 4:300ced917633 | 42 | const float l_OB=.040; |
laskowsk | 4:300ced917633 | 43 | const float l_AC=.095; |
laskowsk | 4:300ced917633 | 44 | const float l_DE=.095; |
laskowsk | 4:300ced917633 | 45 | |
laskowsk | 4:300ced917633 | 46 | // Timing parameters |
laskowsk | 4:300ced917633 | 47 | float pwm_period_us; |
laskowsk | 4:300ced917633 | 48 | float current_control_period_us; |
laskowsk | 4:300ced917633 | 49 | float impedance_control_period_us; |
laskowsk | 4:300ced917633 | 50 | float exp_period; |
laskowsk | 4:300ced917633 | 51 | float omega; |
laskowsk | 4:300ced917633 | 52 | float phase; |
laskowsk | 4:300ced917633 | 53 | |
laskowsk | 4:300ced917633 | 54 | // Control parameters |
laskowsk | 4:300ced917633 | 55 | float K_1; |
laskowsk | 4:300ced917633 | 56 | float K_2; |
laskowsk | 4:300ced917633 | 57 | |
laskowsk | 4:300ced917633 | 58 | float D_1; |
laskowsk | 4:300ced917633 | 59 | float D_2; |
laskowsk | 4:300ced917633 | 60 | |
laskowsk | 4:300ced917633 | 61 | float current_gain; |
laskowsk | 4:300ced917633 | 62 | |
laskowsk | 4:300ced917633 | 63 | float xDesFoot; |
laskowsk | 4:300ced917633 | 64 | float yDesFoot; |
laskowsk | 4:300ced917633 | 65 | |
pwensing | 0:43448bf056e8 | 66 | |
laskowsk | 4:300ced917633 | 67 | // Model parameters |
laskowsk | 4:300ced917633 | 68 | float R; |
laskowsk | 4:300ced917633 | 69 | float k_emf; |
laskowsk | 4:300ced917633 | 70 | float nu1, nu2; |
laskowsk | 4:300ced917633 | 71 | float supply_voltage; |
laskowsk | 4:300ced917633 | 72 | float duty_max; |
laskowsk | 4:300ced917633 | 73 | |
laskowsk | 4:300ced917633 | 74 | |
laskowsk | 4:300ced917633 | 75 | DigitalOut motorFwd1(D7); |
laskowsk | 4:300ced917633 | 76 | DigitalOut motorRev1(D6); |
laskowsk | 4:300ced917633 | 77 | AnalogIn currentSense1(A0); |
laskowsk | 4:300ced917633 | 78 | PwmOut pwmOut1(D5); |
laskowsk | 4:300ced917633 | 79 | |
laskowsk | 4:300ced917633 | 80 | DigitalOut motorFwd2(D13); |
laskowsk | 4:300ced917633 | 81 | DigitalOut motorRev2(D12); |
laskowsk | 4:300ced917633 | 82 | AnalogIn currentSense2(A1); |
laskowsk | 4:300ced917633 | 83 | PwmOut pwmOut2(D11); |
laskowsk | 4:300ced917633 | 84 | |
laskowsk | 4:300ced917633 | 85 | QEI encoder1(D3,D4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding |
laskowsk | 4:300ced917633 | 86 | QEI encoder2(D9,D10, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding |
laskowsk | 4:300ced917633 | 87 | //HX711 load_cell(D0,D1,uint8_t 120); |
laskowsk | 4:300ced917633 | 88 | Ticker currentLoop; |
laskowsk | 4:300ced917633 | 89 | |
laskowsk | 4:300ced917633 | 90 | float prev_current_des1 = 0; |
laskowsk | 4:300ced917633 | 91 | float prev_current_des2 = 0; |
laskowsk | 4:300ced917633 | 92 | void CurrentLoop() |
laskowsk | 4:300ced917633 | 93 | { |
laskowsk | 4:300ced917633 | 94 | motorFwd1 = current_des1 >= 0; |
laskowsk | 4:300ced917633 | 95 | motorRev1 = current_des1 < 0; |
laskowsk | 4:300ced917633 | 96 | |
laskowsk | 4:300ced917633 | 97 | current1 = currentSense1.read()*3.3f / 0.14f; //measure current |
laskowsk | 4:300ced917633 | 98 | if (prev_current_des1 < 0) |
laskowsk | 4:300ced917633 | 99 | current1*=-1; |
laskowsk | 4:300ced917633 | 100 | |
laskowsk | 4:300ced917633 | 101 | duty_factor1=(current_des1*R + current_gain*(current_des1-current1) + k_emf*velocity1)/supply_voltage; |
laskowsk | 4:300ced917633 | 102 | motorRev1 = duty_factor1< 0; |
laskowsk | 4:300ced917633 | 103 | motorFwd1 = duty_factor1>=0; |
laskowsk | 4:300ced917633 | 104 | float absDuty1 = abs(duty_factor1); |
laskowsk | 4:300ced917633 | 105 | if (absDuty1 > duty_max) { |
laskowsk | 4:300ced917633 | 106 | duty_factor1 *= duty_max / absDuty1; |
laskowsk | 4:300ced917633 | 107 | absDuty1= duty_max; |
laskowsk | 4:300ced917633 | 108 | } |
laskowsk | 4:300ced917633 | 109 | pwmOut1.write(absDuty1); |
laskowsk | 4:300ced917633 | 110 | prev_current_des1 = current_des1; |
laskowsk | 4:300ced917633 | 111 | |
laskowsk | 4:300ced917633 | 112 | motorFwd2 = current_des2 >= 0; |
laskowsk | 4:300ced917633 | 113 | motorRev2 = current_des2 < 0; |
laskowsk | 4:300ced917633 | 114 | |
laskowsk | 4:300ced917633 | 115 | current2 = currentSense2.read()*3.3f / 0.14f; //measure current |
laskowsk | 4:300ced917633 | 116 | if (prev_current_des2 < 0) |
laskowsk | 4:300ced917633 | 117 | current2*=-1; |
laskowsk | 4:300ced917633 | 118 | |
laskowsk | 4:300ced917633 | 119 | duty_factor2=(current_des2*R + current_gain*(current_des2-current2) + k_emf*velocity2)/supply_voltage; |
laskowsk | 4:300ced917633 | 120 | |
laskowsk | 4:300ced917633 | 121 | motorRev2 = duty_factor2< 0; |
laskowsk | 4:300ced917633 | 122 | motorFwd2 = duty_factor2>=0; |
laskowsk | 4:300ced917633 | 123 | float absDuty2 = abs(duty_factor2); |
laskowsk | 4:300ced917633 | 124 | if (absDuty2 > duty_max) { |
laskowsk | 4:300ced917633 | 125 | duty_factor2 *= duty_max / absDuty2; |
laskowsk | 4:300ced917633 | 126 | absDuty2= duty_max; |
laskowsk | 4:300ced917633 | 127 | } |
laskowsk | 4:300ced917633 | 128 | pwmOut2.write(absDuty2); |
laskowsk | 4:300ced917633 | 129 | prev_current_des2 = current_des2; |
laskowsk | 4:300ced917633 | 130 | } |
pwensing | 0:43448bf056e8 | 131 | |
pwensing | 0:43448bf056e8 | 132 | int main (void) { |
laskowsk | 4:300ced917633 | 133 | |
laskowsk | 4:300ced917633 | 134 | encoder1.reset(); |
laskowsk | 4:300ced917633 | 135 | encoder2.reset(); |
laskowsk | 4:300ced917633 | 136 | |
laskowsk | 4:300ced917633 | 137 | // Link the terminal with our server and start it up |
laskowsk | 4:300ced917633 | 138 | server.attachTerminal(pc); |
laskowsk | 4:300ced917633 | 139 | server.init(); |
laskowsk | 4:300ced917633 | 140 | |
laskowsk | 4:300ced917633 | 141 | // Continually get input from MATLAB and run experiments |
laskowsk | 4:300ced917633 | 142 | float input_params[NUM_INPUTS]; |
laskowsk | 4:300ced917633 | 143 | while(1) { |
laskowsk | 4:300ced917633 | 144 | if (server.getParams(input_params,NUM_INPUTS)) { |
laskowsk | 4:300ced917633 | 145 | pwm_period_us = input_params[0]; // PWM_Period in mirco seconds |
laskowsk | 4:300ced917633 | 146 | current_control_period_us = input_params[1]; // Current control period in micro seconds |
laskowsk | 4:300ced917633 | 147 | impedance_control_period_us = input_params[2]; // Impedance control period in microseconds seconds |
laskowsk | 4:300ced917633 | 148 | exp_period = input_params[3]; // Experiment time in seconds |
laskowsk | 4:300ced917633 | 149 | |
laskowsk | 4:300ced917633 | 150 | R = input_params[4]; // Terminal resistance (Ohms) |
laskowsk | 4:300ced917633 | 151 | k_emf = input_params[5]; // Back EMF Constant (V / (rad/s)) |
laskowsk | 4:300ced917633 | 152 | nu1 = input_params[6]; // Friction coefficienct 1 (Nm / (rad/s)) |
laskowsk | 4:300ced917633 | 153 | nu2 = input_params[7]; // Friction coefficienct 1 (Nm / (rad/s)) |
laskowsk | 4:300ced917633 | 154 | supply_voltage = input_params[8]; // Power Supply Voltage (V) |
laskowsk | 4:300ced917633 | 155 | |
laskowsk | 4:300ced917633 | 156 | angle1_init = input_params[9]; // Initial angle for q1 (rad) |
laskowsk | 4:300ced917633 | 157 | angle2_init = input_params[10];// Initial angle for q2 (rad) |
laskowsk | 4:300ced917633 | 158 | |
laskowsk | 4:300ced917633 | 159 | current_gain = input_params[11]; // Proportional current gain (V/A) |
laskowsk | 4:300ced917633 | 160 | K_1 = input_params[12]; // Foot stiffness N/m |
laskowsk | 4:300ced917633 | 161 | K_2 = input_params[13]; // Foot stiffness N/m |
laskowsk | 4:300ced917633 | 162 | max_angle1 = input_params[14]; // Foot stiffness N/m |
pwensing | 0:43448bf056e8 | 163 | |
laskowsk | 4:300ced917633 | 164 | D_1 = input_params[15]; // Foot damping N/(m/s) |
laskowsk | 4:300ced917633 | 165 | D_2 = input_params[16]; // Foot damping N/(m/s) |
laskowsk | 4:300ced917633 | 166 | max_angle2 = input_params[17]; // Foot damping N/(m/s) |
laskowsk | 4:300ced917633 | 167 | duty_max = input_params[18]; // Maximum duty factor |
laskowsk | 4:300ced917633 | 168 | omega = input_params[19]; // Oscillation freq of the tail |
laskowsk | 4:300ced917633 | 169 | phase = input_params[20]; // phase difference between two motors |
laskowsk | 4:300ced917633 | 170 | |
laskowsk | 4:300ced917633 | 171 | pwmOut1.period_us(pwm_period_us); |
laskowsk | 4:300ced917633 | 172 | pwmOut2.period_us(pwm_period_us); |
laskowsk | 4:300ced917633 | 173 | float m1 =.02 + .210; |
laskowsk | 4:300ced917633 | 174 | float m2 =.0225; |
laskowsk | 4:300ced917633 | 175 | float m3 = .004; |
laskowsk | 4:300ced917633 | 176 | float m4 = .017; |
laskowsk | 4:300ced917633 | 177 | //float I1 = 45.389 * 10^-6; |
laskowsk | 4:300ced917633 | 178 | //float I2 = 22.918 * 10^-6; |
laskowsk | 4:300ced917633 | 179 | //float I3 = 3.2570 * 10^-6; |
laskowsk | 4:300ced917633 | 180 | //float I4 = 22.176 * 10^-6; |
laskowsk | 4:300ced917633 | 181 | float l_OA=.011; |
laskowsk | 4:300ced917633 | 182 | float l_OB=.042; |
laskowsk | 4:300ced917633 | 183 | float l_AC=.096; |
laskowsk | 4:300ced917633 | 184 | float l_DE=.096; |
laskowsk | 4:300ced917633 | 185 | float l_O_m1=0.0364; |
laskowsk | 4:300ced917633 | 186 | float l_B_m2=0.040; |
laskowsk | 4:300ced917633 | 187 | float l_A_m3=1/2 * l_AC; |
laskowsk | 4:300ced917633 | 188 | float l_C_m4=1/2 * (l_DE+ l_OB-l_OA); |
laskowsk | 4:300ced917633 | 189 | float g = 9.81; |
laskowsk | 4:300ced917633 | 190 | |
laskowsk | 4:300ced917633 | 191 | // Attach current loop! |
laskowsk | 4:300ced917633 | 192 | currentLoop.attach_us(CurrentLoop,current_control_period_us); |
laskowsk | 4:300ced917633 | 193 | |
laskowsk | 4:300ced917633 | 194 | // Setup experiment |
laskowsk | 4:300ced917633 | 195 | t.reset(); |
laskowsk | 4:300ced917633 | 196 | t.start(); |
pwensing | 0:43448bf056e8 | 197 | |
laskowsk | 4:300ced917633 | 198 | motorFwd1 = 1; |
laskowsk | 4:300ced917633 | 199 | motorRev1 = 0; |
laskowsk | 4:300ced917633 | 200 | pwmOut1.write(0); |
laskowsk | 4:300ced917633 | 201 | |
laskowsk | 4:300ced917633 | 202 | motorFwd2 = 1; |
laskowsk | 4:300ced917633 | 203 | motorRev2 = 0; |
laskowsk | 4:300ced917633 | 204 | pwmOut2.write(0); |
laskowsk | 4:300ced917633 | 205 | |
laskowsk | 4:300ced917633 | 206 | // Run experiment |
laskowsk | 4:300ced917633 | 207 | while( t.read() < exp_period ) { |
laskowsk | 4:300ced917633 | 208 | // Perform control loop logic |
laskowsk | 4:300ced917633 | 209 | |
laskowsk | 4:300ced917633 | 210 | angle1 = encoder1.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor |
laskowsk | 4:300ced917633 | 211 | dif_ang1 = angle1 - max_angle1*sin(omega*t.read()); // calc difference of actual and desired angle |
laskowsk | 4:300ced917633 | 212 | velocity1 = encoder1.getVelocity() * PULSE_TO_RAD; |
laskowsk | 4:300ced917633 | 213 | |
laskowsk | 4:300ced917633 | 214 | angle2 = encoder1.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor |
laskowsk | 4:300ced917633 | 215 | dif_ang2 = angle2 - max_angle2*sin(omega*t.read()+phase); // calc difference of actual and desired angle |
laskowsk | 4:300ced917633 | 216 | velocity2 = encoder2.getVelocity() * PULSE_TO_RAD; |
laskowsk | 3:c8e53b5762bd | 217 | |
laskowsk | 4:300ced917633 | 218 | // Forward Kinematics |
laskowsk | 4:300ced917633 | 219 | |
laskowsk | 4:300ced917633 | 220 | float tau_des1 = (-K_1*dif_ang1-D_1*velocity1+nu1*velocity1); |
laskowsk | 4:300ced917633 | 221 | float tau_des2 = (-K_1*dif_ang2-D_1*velocity2+nu2*velocity2); |
laskowsk | 4:300ced917633 | 222 | |
laskowsk | 4:300ced917633 | 223 | // Set desired currents |
laskowsk | 4:300ced917633 | 224 | current_des1 = tau_des1/k_emf;//(-K_xx*angle1-D_xx*velocity1+nu1*velocity1)/k_emf; |
laskowsk | 4:300ced917633 | 225 | current_des2 = tau_des2/k_emf; //(-K_yy*angle2-D_yy*velocity2+nu2*velocity2)/k_emf; |
laskowsk | 4:300ced917633 | 226 | |
laskowsk | 4:300ced917633 | 227 | // Form output to send to MATLAB |
laskowsk | 4:300ced917633 | 228 | float output_data[NUM_OUTPUTS]; |
laskowsk | 4:300ced917633 | 229 | output_data[0] = t.read(); |
laskowsk | 4:300ced917633 | 230 | output_data[1] = angle1; |
laskowsk | 4:300ced917633 | 231 | output_data[2] = velocity1; |
laskowsk | 4:300ced917633 | 232 | output_data[3] = current1; |
laskowsk | 4:300ced917633 | 233 | output_data[4] = current_des1; |
laskowsk | 4:300ced917633 | 234 | output_data[5] = duty_factor1; |
laskowsk | 4:300ced917633 | 235 | |
laskowsk | 4:300ced917633 | 236 | output_data[6] = angle2; |
laskowsk | 4:300ced917633 | 237 | output_data[7] = velocity2; |
laskowsk | 4:300ced917633 | 238 | output_data[8] = current2; |
laskowsk | 4:300ced917633 | 239 | output_data[9] = current_des2; |
laskowsk | 4:300ced917633 | 240 | output_data[10]= duty_factor2; |
laskowsk | 4:300ced917633 | 241 | |
laskowsk | 4:300ced917633 | 242 | output_data[11] = tau_des1; |
laskowsk | 4:300ced917633 | 243 | output_data[12] = tau_des2; |
laskowsk | 4:300ced917633 | 244 | output_data[13] = dif_ang1; |
laskowsk | 4:300ced917633 | 245 | output_data[14] = dif_ang2; |
laskowsk | 4:300ced917633 | 246 | //output_data[15] = fx; |
laskowsk | 4:300ced917633 | 247 | //output_data[16] = fy; |
laskowsk | 4:300ced917633 | 248 | |
laskowsk | 4:300ced917633 | 249 | // Send data to MATLAB |
laskowsk | 4:300ced917633 | 250 | server.sendData(output_data,NUM_OUTPUTS); |
laskowsk | 4:300ced917633 | 251 | wait_us(impedance_control_period_us); |
laskowsk | 4:300ced917633 | 252 | } |
laskowsk | 4:300ced917633 | 253 | // Cleanup after experiment |
laskowsk | 4:300ced917633 | 254 | server.setExperimentComplete(); |
laskowsk | 4:300ced917633 | 255 | // control.detach(); |
laskowsk | 4:300ced917633 | 256 | currentLoop.detach(); |
laskowsk | 4:300ced917633 | 257 | pwmOut1.write(0); |
laskowsk | 4:300ced917633 | 258 | pwmOut2.write(0); |
laskowsk | 4:300ced917633 | 259 | |
laskowsk | 4:300ced917633 | 260 | } // end if |
laskowsk | 4:300ced917633 | 261 | } // end while |
laskowsk | 4:300ced917633 | 262 | } // end main |