Stephen Laskowski / Mbed 2 deprecated Dolphin

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 Patrick Wensing

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?

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