![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2.74 Bio-Inspired Robotics robot for LGO group. This is the vertical dolphin tail
Dependencies: EthernetInterface ExperimentServer HX711 QEI_pmw mbed-rtos mbed
Fork of experiment_example by
main.cpp@6:b7f6433cc765, 2015-11-30 (annotated)
- Committer:
- laskowsk
- Date:
- Mon Nov 30 18:29:56 2015 +0000
- Revision:
- 6:b7f6433cc765
- Parent:
- 4:300ced917633
commit
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 | 6:b7f6433cc765 | 8 | #define NUM_INPUTS 22 |
laskowsk | 6:b7f6433cc765 | 9 | #define NUM_OUTPUTS 16 |
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 | // Timing parameters |
laskowsk | 4:300ced917633 | 41 | float pwm_period_us; |
laskowsk | 4:300ced917633 | 42 | float current_control_period_us; |
laskowsk | 4:300ced917633 | 43 | float impedance_control_period_us; |
laskowsk | 4:300ced917633 | 44 | float exp_period; |
laskowsk | 4:300ced917633 | 45 | float omega; |
laskowsk | 4:300ced917633 | 46 | float phase; |
laskowsk | 4:300ced917633 | 47 | |
laskowsk | 4:300ced917633 | 48 | // Control parameters |
laskowsk | 4:300ced917633 | 49 | float K_1; |
laskowsk | 4:300ced917633 | 50 | float K_2; |
laskowsk | 4:300ced917633 | 51 | |
laskowsk | 4:300ced917633 | 52 | float D_1; |
laskowsk | 4:300ced917633 | 53 | float D_2; |
laskowsk | 4:300ced917633 | 54 | |
laskowsk | 4:300ced917633 | 55 | float current_gain; |
laskowsk | 6:b7f6433cc765 | 56 | float calibration_factor; |
laskowsk | 4:300ced917633 | 57 | |
laskowsk | 4:300ced917633 | 58 | float xDesFoot; |
laskowsk | 4:300ced917633 | 59 | float yDesFoot; |
laskowsk | 4:300ced917633 | 60 | |
pwensing | 0:43448bf056e8 | 61 | |
laskowsk | 4:300ced917633 | 62 | // Model parameters |
laskowsk | 4:300ced917633 | 63 | float R; |
laskowsk | 4:300ced917633 | 64 | float k_emf; |
laskowsk | 4:300ced917633 | 65 | float nu1, nu2; |
laskowsk | 4:300ced917633 | 66 | float supply_voltage; |
laskowsk | 4:300ced917633 | 67 | float duty_max; |
laskowsk | 4:300ced917633 | 68 | |
laskowsk | 4:300ced917633 | 69 | |
laskowsk | 4:300ced917633 | 70 | DigitalOut motorFwd1(D7); |
laskowsk | 4:300ced917633 | 71 | DigitalOut motorRev1(D6); |
laskowsk | 4:300ced917633 | 72 | AnalogIn currentSense1(A0); |
laskowsk | 4:300ced917633 | 73 | PwmOut pwmOut1(D5); |
laskowsk | 4:300ced917633 | 74 | |
laskowsk | 4:300ced917633 | 75 | DigitalOut motorFwd2(D13); |
laskowsk | 4:300ced917633 | 76 | DigitalOut motorRev2(D12); |
laskowsk | 4:300ced917633 | 77 | AnalogIn currentSense2(A1); |
laskowsk | 4:300ced917633 | 78 | PwmOut pwmOut2(D11); |
laskowsk | 4:300ced917633 | 79 | |
laskowsk | 4:300ced917633 | 80 | QEI encoder1(D3,D4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding |
laskowsk | 4:300ced917633 | 81 | QEI encoder2(D9,D10, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding |
laskowsk | 6:b7f6433cc765 | 82 | HX711 scale(A0,A1); |
laskowsk | 6:b7f6433cc765 | 83 | |
laskowsk | 4:300ced917633 | 84 | Ticker currentLoop; |
laskowsk | 4:300ced917633 | 85 | |
laskowsk | 6:b7f6433cc765 | 86 | |
laskowsk | 4:300ced917633 | 87 | float prev_current_des1 = 0; |
laskowsk | 4:300ced917633 | 88 | float prev_current_des2 = 0; |
laskowsk | 4:300ced917633 | 89 | void CurrentLoop() |
laskowsk | 4:300ced917633 | 90 | { |
laskowsk | 4:300ced917633 | 91 | motorFwd1 = current_des1 >= 0; |
laskowsk | 4:300ced917633 | 92 | motorRev1 = current_des1 < 0; |
laskowsk | 4:300ced917633 | 93 | |
laskowsk | 4:300ced917633 | 94 | current1 = currentSense1.read()*3.3f / 0.14f; //measure current |
laskowsk | 4:300ced917633 | 95 | if (prev_current_des1 < 0) |
laskowsk | 4:300ced917633 | 96 | current1*=-1; |
laskowsk | 4:300ced917633 | 97 | |
laskowsk | 4:300ced917633 | 98 | duty_factor1=(current_des1*R + current_gain*(current_des1-current1) + k_emf*velocity1)/supply_voltage; |
laskowsk | 4:300ced917633 | 99 | motorRev1 = duty_factor1< 0; |
laskowsk | 4:300ced917633 | 100 | motorFwd1 = duty_factor1>=0; |
laskowsk | 4:300ced917633 | 101 | float absDuty1 = abs(duty_factor1); |
laskowsk | 4:300ced917633 | 102 | if (absDuty1 > duty_max) { |
laskowsk | 4:300ced917633 | 103 | duty_factor1 *= duty_max / absDuty1; |
laskowsk | 4:300ced917633 | 104 | absDuty1= duty_max; |
laskowsk | 4:300ced917633 | 105 | } |
laskowsk | 4:300ced917633 | 106 | pwmOut1.write(absDuty1); |
laskowsk | 4:300ced917633 | 107 | prev_current_des1 = current_des1; |
laskowsk | 4:300ced917633 | 108 | |
laskowsk | 4:300ced917633 | 109 | motorFwd2 = current_des2 >= 0; |
laskowsk | 4:300ced917633 | 110 | motorRev2 = current_des2 < 0; |
laskowsk | 4:300ced917633 | 111 | |
laskowsk | 4:300ced917633 | 112 | current2 = currentSense2.read()*3.3f / 0.14f; //measure current |
laskowsk | 4:300ced917633 | 113 | if (prev_current_des2 < 0) |
laskowsk | 4:300ced917633 | 114 | current2*=-1; |
laskowsk | 4:300ced917633 | 115 | |
laskowsk | 4:300ced917633 | 116 | duty_factor2=(current_des2*R + current_gain*(current_des2-current2) + k_emf*velocity2)/supply_voltage; |
laskowsk | 4:300ced917633 | 117 | |
laskowsk | 4:300ced917633 | 118 | motorRev2 = duty_factor2< 0; |
laskowsk | 4:300ced917633 | 119 | motorFwd2 = duty_factor2>=0; |
laskowsk | 4:300ced917633 | 120 | float absDuty2 = abs(duty_factor2); |
laskowsk | 4:300ced917633 | 121 | if (absDuty2 > duty_max) { |
laskowsk | 4:300ced917633 | 122 | duty_factor2 *= duty_max / absDuty2; |
laskowsk | 4:300ced917633 | 123 | absDuty2= duty_max; |
laskowsk | 4:300ced917633 | 124 | } |
laskowsk | 4:300ced917633 | 125 | pwmOut2.write(absDuty2); |
laskowsk | 4:300ced917633 | 126 | prev_current_des2 = current_des2; |
laskowsk | 4:300ced917633 | 127 | } |
pwensing | 0:43448bf056e8 | 128 | |
pwensing | 0:43448bf056e8 | 129 | int main (void) { |
laskowsk | 6:b7f6433cc765 | 130 | scale.tare(); // tare scale |
laskowsk | 4:300ced917633 | 131 | encoder1.reset(); |
laskowsk | 4:300ced917633 | 132 | encoder2.reset(); |
laskowsk | 4:300ced917633 | 133 | |
laskowsk | 4:300ced917633 | 134 | // Link the terminal with our server and start it up |
laskowsk | 4:300ced917633 | 135 | server.attachTerminal(pc); |
laskowsk | 4:300ced917633 | 136 | server.init(); |
laskowsk | 4:300ced917633 | 137 | |
laskowsk | 4:300ced917633 | 138 | // Continually get input from MATLAB and run experiments |
laskowsk | 4:300ced917633 | 139 | float input_params[NUM_INPUTS]; |
laskowsk | 4:300ced917633 | 140 | while(1) { |
laskowsk | 4:300ced917633 | 141 | if (server.getParams(input_params,NUM_INPUTS)) { |
laskowsk | 4:300ced917633 | 142 | pwm_period_us = input_params[0]; // PWM_Period in mirco seconds |
laskowsk | 4:300ced917633 | 143 | current_control_period_us = input_params[1]; // Current control period in micro seconds |
laskowsk | 4:300ced917633 | 144 | impedance_control_period_us = input_params[2]; // Impedance control period in microseconds seconds |
laskowsk | 4:300ced917633 | 145 | exp_period = input_params[3]; // Experiment time in seconds |
laskowsk | 4:300ced917633 | 146 | |
laskowsk | 4:300ced917633 | 147 | R = input_params[4]; // Terminal resistance (Ohms) |
laskowsk | 4:300ced917633 | 148 | k_emf = input_params[5]; // Back EMF Constant (V / (rad/s)) |
laskowsk | 4:300ced917633 | 149 | nu1 = input_params[6]; // Friction coefficienct 1 (Nm / (rad/s)) |
laskowsk | 4:300ced917633 | 150 | nu2 = input_params[7]; // Friction coefficienct 1 (Nm / (rad/s)) |
laskowsk | 4:300ced917633 | 151 | supply_voltage = input_params[8]; // Power Supply Voltage (V) |
laskowsk | 4:300ced917633 | 152 | |
laskowsk | 4:300ced917633 | 153 | angle1_init = input_params[9]; // Initial angle for q1 (rad) |
laskowsk | 4:300ced917633 | 154 | angle2_init = input_params[10];// Initial angle for q2 (rad) |
laskowsk | 4:300ced917633 | 155 | |
laskowsk | 4:300ced917633 | 156 | current_gain = input_params[11]; // Proportional current gain (V/A) |
laskowsk | 4:300ced917633 | 157 | K_1 = input_params[12]; // Foot stiffness N/m |
laskowsk | 4:300ced917633 | 158 | K_2 = input_params[13]; // Foot stiffness N/m |
laskowsk | 4:300ced917633 | 159 | max_angle1 = input_params[14]; // Foot stiffness N/m |
pwensing | 0:43448bf056e8 | 160 | |
laskowsk | 4:300ced917633 | 161 | D_1 = input_params[15]; // Foot damping N/(m/s) |
laskowsk | 4:300ced917633 | 162 | D_2 = input_params[16]; // Foot damping N/(m/s) |
laskowsk | 4:300ced917633 | 163 | max_angle2 = input_params[17]; // Foot damping N/(m/s) |
laskowsk | 4:300ced917633 | 164 | duty_max = input_params[18]; // Maximum duty factor |
laskowsk | 4:300ced917633 | 165 | omega = input_params[19]; // Oscillation freq of the tail |
laskowsk | 4:300ced917633 | 166 | phase = input_params[20]; // phase difference between two motors |
laskowsk | 6:b7f6433cc765 | 167 | calibration_factor = input_params[21]; // calibration factor for load cell |
laskowsk | 6:b7f6433cc765 | 168 | |
laskowsk | 4:300ced917633 | 169 | pwmOut1.period_us(pwm_period_us); |
laskowsk | 4:300ced917633 | 170 | pwmOut2.period_us(pwm_period_us); |
laskowsk | 6:b7f6433cc765 | 171 | scale.setScale(calibration_factor); //Adjust to this calibration factor |
laskowsk | 6:b7f6433cc765 | 172 | |
laskowsk | 4:300ced917633 | 173 | |
laskowsk | 4:300ced917633 | 174 | // Attach current loop! |
laskowsk | 4:300ced917633 | 175 | currentLoop.attach_us(CurrentLoop,current_control_period_us); |
laskowsk | 4:300ced917633 | 176 | |
laskowsk | 4:300ced917633 | 177 | // Setup experiment |
laskowsk | 4:300ced917633 | 178 | t.reset(); |
laskowsk | 4:300ced917633 | 179 | t.start(); |
pwensing | 0:43448bf056e8 | 180 | |
laskowsk | 4:300ced917633 | 181 | motorFwd1 = 1; |
laskowsk | 4:300ced917633 | 182 | motorRev1 = 0; |
laskowsk | 4:300ced917633 | 183 | pwmOut1.write(0); |
laskowsk | 4:300ced917633 | 184 | |
laskowsk | 4:300ced917633 | 185 | motorFwd2 = 1; |
laskowsk | 4:300ced917633 | 186 | motorRev2 = 0; |
laskowsk | 4:300ced917633 | 187 | pwmOut2.write(0); |
laskowsk | 4:300ced917633 | 188 | |
laskowsk | 4:300ced917633 | 189 | // Run experiment |
laskowsk | 4:300ced917633 | 190 | while( t.read() < exp_period ) { |
laskowsk | 4:300ced917633 | 191 | // Perform control loop logic |
laskowsk | 4:300ced917633 | 192 | |
laskowsk | 4:300ced917633 | 193 | angle1 = encoder1.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor |
laskowsk | 4:300ced917633 | 194 | dif_ang1 = angle1 - max_angle1*sin(omega*t.read()); // calc difference of actual and desired angle |
laskowsk | 4:300ced917633 | 195 | velocity1 = encoder1.getVelocity() * PULSE_TO_RAD; |
laskowsk | 4:300ced917633 | 196 | |
laskowsk | 6:b7f6433cc765 | 197 | angle2 = encoder2.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor |
laskowsk | 4:300ced917633 | 198 | dif_ang2 = angle2 - max_angle2*sin(omega*t.read()+phase); // calc difference of actual and desired angle |
laskowsk | 4:300ced917633 | 199 | velocity2 = encoder2.getVelocity() * PULSE_TO_RAD; |
laskowsk | 3:c8e53b5762bd | 200 | |
laskowsk | 4:300ced917633 | 201 | // Forward Kinematics |
laskowsk | 4:300ced917633 | 202 | |
laskowsk | 4:300ced917633 | 203 | float tau_des1 = (-K_1*dif_ang1-D_1*velocity1+nu1*velocity1); |
laskowsk | 6:b7f6433cc765 | 204 | float tau_des2 = (-K_2*dif_ang2-D_2*velocity2+nu2*velocity2); |
laskowsk | 4:300ced917633 | 205 | |
laskowsk | 4:300ced917633 | 206 | // Set desired currents |
laskowsk | 4:300ced917633 | 207 | current_des1 = tau_des1/k_emf;//(-K_xx*angle1-D_xx*velocity1+nu1*velocity1)/k_emf; |
laskowsk | 6:b7f6433cc765 | 208 | current_des2 = tau_des2/k_emf; //(-K_yy*angle2-D_yy*velocity2+nu2*velocity2)/k_emf; |
laskowsk | 6:b7f6433cc765 | 209 | |
laskowsk | 6:b7f6433cc765 | 210 | // take measurement from load cell |
laskowsk | 6:b7f6433cc765 | 211 | float F = scale.getGram(); // value from load cell that uses calibration factor |
laskowsk | 4:300ced917633 | 212 | |
laskowsk | 4:300ced917633 | 213 | // Form output to send to MATLAB |
laskowsk | 4:300ced917633 | 214 | float output_data[NUM_OUTPUTS]; |
laskowsk | 4:300ced917633 | 215 | output_data[0] = t.read(); |
laskowsk | 4:300ced917633 | 216 | output_data[1] = angle1; |
laskowsk | 4:300ced917633 | 217 | output_data[2] = velocity1; |
laskowsk | 4:300ced917633 | 218 | output_data[3] = current1; |
laskowsk | 4:300ced917633 | 219 | output_data[4] = current_des1; |
laskowsk | 4:300ced917633 | 220 | output_data[5] = duty_factor1; |
laskowsk | 4:300ced917633 | 221 | |
laskowsk | 4:300ced917633 | 222 | output_data[6] = angle2; |
laskowsk | 4:300ced917633 | 223 | output_data[7] = velocity2; |
laskowsk | 4:300ced917633 | 224 | output_data[8] = current2; |
laskowsk | 4:300ced917633 | 225 | output_data[9] = current_des2; |
laskowsk | 4:300ced917633 | 226 | output_data[10]= duty_factor2; |
laskowsk | 4:300ced917633 | 227 | |
laskowsk | 4:300ced917633 | 228 | output_data[11] = tau_des1; |
laskowsk | 4:300ced917633 | 229 | output_data[12] = tau_des2; |
laskowsk | 4:300ced917633 | 230 | output_data[13] = dif_ang1; |
laskowsk | 4:300ced917633 | 231 | output_data[14] = dif_ang2; |
laskowsk | 6:b7f6433cc765 | 232 | output_data[15] = F; |
laskowsk | 4:300ced917633 | 233 | //output_data[16] = fy; |
laskowsk | 4:300ced917633 | 234 | |
laskowsk | 4:300ced917633 | 235 | // Send data to MATLAB |
laskowsk | 4:300ced917633 | 236 | server.sendData(output_data,NUM_OUTPUTS); |
laskowsk | 4:300ced917633 | 237 | wait_us(impedance_control_period_us); |
laskowsk | 4:300ced917633 | 238 | } |
laskowsk | 4:300ced917633 | 239 | // Cleanup after experiment |
laskowsk | 4:300ced917633 | 240 | server.setExperimentComplete(); |
laskowsk | 4:300ced917633 | 241 | // control.detach(); |
laskowsk | 4:300ced917633 | 242 | currentLoop.detach(); |
laskowsk | 4:300ced917633 | 243 | pwmOut1.write(0); |
laskowsk | 4:300ced917633 | 244 | pwmOut2.write(0); |
laskowsk | 4:300ced917633 | 245 | |
laskowsk | 4:300ced917633 | 246 | } // end if |
laskowsk | 4:300ced917633 | 247 | } // end while |
laskowsk | 4:300ced917633 | 248 | } // end main |