Stephen Laskowski / Mbed 2 deprecated 2_131TEST

2.131 test rig DAQ for MTB suspension characterization project

Dependencies:   EthernetInterface ExperimentServer HX711 QEI_pmw mbed-rtos mbed

Fork of Dolphin by Stephen Laskowski

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?

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"
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