Jochem Brouwer / Mbed 2 deprecated PID_controller_Motor2

MODSERIAL PID controller edit

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of PID_controller_Motor by BioBitches TN

main.cpp@30:457e42514c47, 2016-10-28 (annotated)

Committer:
MBroek
Date:
Fri Oct 28 09:19:48 2016 +0000
Revision:
30:457e42514c47
Parent:
29:2b711fc9a5b2 
robot testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MBroek 0:9e053ed05c69 1
MBroek 0:9e053ed05c69 2
MBroek 0:9e053ed05c69 3 // HET DEFINIEREN VAN ALLES ==========================================================================================
MBroek 0:9e053ed05c69 4
MBroek 0:9e053ed05c69 5 // Includen van alle libraries ---------------------------------------
MBroek 0:9e053ed05c69 6 #include "mbed.h"
MBroek 0:9e053ed05c69 7 #include "MODSERIAL.h"
MBroek 0:9e053ed05c69 8 #include "QEI.h"
MBroek 0:9e053ed05c69 9 #include "math.h"
MBroek 3:581c5918b590 10 #include "HIDScope.h"
MBroek 24:ea6de9237cff 11 #include "BiQuad.h"
MBroek 3:581c5918b590 12
MBroek 3:581c5918b590 13 // Definieren van de HIDscope ----------------------------------------
MBroek 29:2b711fc9a5b2 14 HIDScope scope(3);
MBroek 0:9e053ed05c69 15
MBroek 0:9e053ed05c69 16
MBroek 0:9e053ed05c69 17 // Definieren van de Serial en Encoder -------------------------------
MBroek 0:9e053ed05c69 18 MODSERIAL pc(USBTX, USBRX);
MBroek 0:9e053ed05c69 19 #define SERIALBAUD 115200
MBroek 0:9e053ed05c69 20
MBroek 28:97b9e50c1180 21 QEI encoder_motor1(D10,D11,NC,64, QEI::X4_ENCODING);
MBroek 28:97b9e50c1180 22 QEI encoder_motor2(D12,D13,NC,64, QEI::X4_ENCODING);
MBroek 0:9e053ed05c69 23
MBroek 0:9e053ed05c69 24
MBroek 0:9e053ed05c69 25 // Definieren van de Motorpins ---------------------------------------
MBroek 0:9e053ed05c69 26 DigitalOut motor1_direction_pin(D7);
MBroek 0:9e053ed05c69 27 PwmOut motor1_speed_pin(D6);
MBroek 0:9e053ed05c69 28
MBroek 0:9e053ed05c69 29 DigitalOut motor2_direction_pin(D4);
MBroek 0:9e053ed05c69 30 PwmOut motor2_speed_pin(D5);
MBroek 0:9e053ed05c69 31
MBroek 0:9e053ed05c69 32
MBroek 0:9e053ed05c69 33 //Definieren van bord-elementen --------------------------------------
MBroek 19:35f3da6c6969 34 InterruptIn motor_switch_button(D3);
MBroek 19:35f3da6c6969 35 InterruptIn state_switch_button(D2);
MBroek 17:525b785f007a 36 DigitalIn EMG_sim1(SW2);
MBroek 17:525b785f007a 37 DigitalIn EMG_sim2(SW3);
MBroek 17:525b785f007a 38
MBroek 28:97b9e50c1180 39 //AnalogIn pot1(A1); // Dit is de gesimuleerde emg1
MBroek 28:97b9e50c1180 40 //AnalogIn pot2(A2); // Dit is de gesimuleerde emg2
MBroek 0:9e053ed05c69 41
MBroek 13:b0d3c547bf2f 42
MBroek 13:b0d3c547bf2f 43 DigitalOut ledred(LED_RED, 1);
MBroek 13:b0d3c547bf2f 44 DigitalOut ledgreen(LED_GREEN, 1);
MBroek 16:5a749b319276 45 DigitalOut ledblue(LED_BLUE, 1);
MBroek 13:b0d3c547bf2f 46
MBroek 24:ea6de9237cff 47 AnalogIn emg0( A0 ); // right biceps
MBroek 24:ea6de9237cff 48 AnalogIn emg1( A1 ); // left biceps
MBroek 24:ea6de9237cff 49
MBroek 28:97b9e50c1180 50 AnalogIn init_pot(A3); // POT meter
MBroek 28:97b9e50c1180 51
MBroek 0:9e053ed05c69 52
MBroek 4:6524b198721f 53 //Definieren van de tickers ------------------------------------------
MBroek 17:525b785f007a 54 Ticker motor_ticker; // Deze ticker activeert het motor_runs programma, dit leest motor pos en ref pos uit, rekent de PID uit en stuurt met dit de motoren
MBroek 4:6524b198721f 55 Ticker hidscope_ticker;
MBroek 4:6524b198721f 56
MBroek 4:6524b198721f 57
MBroek 24:ea6de9237cff 58 // Het definieren van de filters ----------------------------------------
MBroek 24:ea6de9237cff 59 BiQuadChain bqc1; // Combined filter: high-pass (10Hz) and notch (50Hz)
MBroek 24:ea6de9237cff 60 BiQuad bq1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
MBroek 24:ea6de9237cff 61 BiQuad bq2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
MBroek 24:ea6de9237cff 62 BiQuad bq3( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
MBroek 24:ea6de9237cff 63
MBroek 24:ea6de9237cff 64 BiQuadChain bqc2;
MBroek 24:ea6de9237cff 65 BiQuad bq4 = bq1;
MBroek 24:ea6de9237cff 66 BiQuad bq5 = bq2;
MBroek 24:ea6de9237cff 67 BiQuad bq6 = bq3;
MBroek 24:ea6de9237cff 68
MBroek 24:ea6de9237cff 69 BiQuadChain bqc3; // Low-pass filter (5Hz)
MBroek 24:ea6de9237cff 70 BiQuad bq7( 5.84514e-08, 1.16903e-07, 5.84514e-08, -1.94264e+00, 9.43597e-01 );
MBroek 24:ea6de9237cff 71 BiQuad bq8( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.97527e+00, 9.76245e-01 );
MBroek 24:ea6de9237cff 72
MBroek 24:ea6de9237cff 73 BiQuadChain bqc4;
MBroek 24:ea6de9237cff 74 BiQuad bq9 = bq7;
MBroek 24:ea6de9237cff 75 BiQuad bq10 = bq9;
MBroek 24:ea6de9237cff 76
MBroek 28:97b9e50c1180 77 BiQuadChain bqc5; // Low-pass filter (10Hz) voor PID
MBroek 28:97b9e50c1180 78 BiQuad bq11( 1.32937e-05, 2.65875e-05, 1.32937e-05, -1.77831e+00, 7.92447e-01 );
MBroek 28:97b9e50c1180 79 BiQuad bq12( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.89342e+00, 9.08464e-01 );
MBroek 28:97b9e50c1180 80
MBroek 28:97b9e50c1180 81
MBroek 28:97b9e50c1180 82
MBroek 28:97b9e50c1180 83
MBroek 24:ea6de9237cff 84
MBroek 0:9e053ed05c69 85
MBroek 0:9e053ed05c69 86 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
MBroek 4:6524b198721f 87
MBroek 28:97b9e50c1180 88 enum states_enum{off, init, init_fish, run}; // De states waar de main loop doorheen loopt, off is uit, init is intialiseren en run is het aanturen vd motoren met emg.
MBroek 14:f51d51395803 89 states_enum states = off;
MBroek 14:f51d51395803 90
MBroek 21:3db3f2d56d56 91 double ref_pos_prev_m1 = 0.0; // De initiele ref_pos_pref
MBroek 21:3db3f2d56d56 92 double ref_pos_prev_m2 = 0.0;
MBroek 21:3db3f2d56d56 93
MBroek 4:6524b198721f 94 int counts1; // Pulses van motoren
MBroek 4:6524b198721f 95 int counts2;
MBroek 0:9e053ed05c69 96
MBroek 0:9e053ed05c69 97 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 15:3f3d87513a6b 98 const double rad_per_count = 2.0*pi/8400.0; // Aantal rad per puls uit encoder
MBroek 0:9e053ed05c69 99
MBroek 23:3902ee714281 100 const double radius_tandwiel = 0.008;
MBroek 9:f735baee0c2b 101 const double meter_per_count = rad_per_count * radius_tandwiel; // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
MBroek 1:5b3fa8e47e8b 102
MBroek 23:3902ee714281 103 const double v_max_karretje = 8.4*radius_tandwiel; // Maximale snelheid karretje, gelimiteerd door de v_max vd motor
MBroek 23:3902ee714281 104
MBroek 4:6524b198721f 105 double error1_int = 0.00000; // Initiele error integral waardes
MBroek 4:6524b198721f 106 double error2_int = 0.00000;
MBroek 0:9e053ed05c69 107
MBroek 30:457e42514c47 108 const double T_motor_function = 0.01; // Periode van de frequentie van het aanroepen van onder andere positiechecker (get_position) en de rest vd motor functie
MBroek 0:9e053ed05c69 109
MBroek 4:6524b198721f 110 volatile bool flag1 = false, flag2 = false; // De flags voor de functies aangeroepen door de tickers
MBroek 4:6524b198721f 111
MBroek 30:457e42514c47 112 const double Kp_1 = 0.2; // De PID variablen voor motor 1
MBroek 30:457e42514c47 113 const double Kd_1 = 0.01;
MBroek 30:457e42514c47 114 const double Ki_1 = 0.01;
MBroek 4:6524b198721f 115
MBroek 23:3902ee714281 116 const double Kp_2 = 1.0; // De PID variablen voor motor 2
MBroek 28:97b9e50c1180 117 const double Kd_2 = 0.4;
MBroek 28:97b9e50c1180 118 const double Ki_2 = 0.1;
MBroek 5:c510ab61af28 119
MBroek 28:97b9e50c1180 120 const double vrijheid_m1_rad = 0.4*pi; // Dit is de uiterste postitie vd arm in radialen
MBroek 29:2b711fc9a5b2 121 const double vrijheid_m2_meter = 0.25; // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
MBroek 5:c510ab61af28 122
MBroek 21:3db3f2d56d56 123 const double initial_pos_m1 = vrijheid_m1_rad; // Startin posities van de motoren
MBroek 28:97b9e50c1180 124 const double initial_pos_m2 = vrijheid_m2_meter;
MBroek 28:97b9e50c1180 125
MBroek 28:97b9e50c1180 126 double position_motor1;
MBroek 23:3902ee714281 127
MBroek 23:3902ee714281 128 double position_motor2;
MBroek 23:3902ee714281 129 double position_motor2_prev = initial_pos_m2;
MBroek 9:f735baee0c2b 130
MBroek 13:b0d3c547bf2f 131 double PID_output_1 = 0.0; // De eerste PID_output, deze is nul anders gaan de motoren draaien
MBroek 13:b0d3c547bf2f 132 double PID_output_2 = 0.0;
MBroek 5:c510ab61af28 133
MBroek 13:b0d3c547bf2f 134 double error_prev_1 = 0.0; // De initiele previous error
MBroek 13:b0d3c547bf2f 135 double error_int_1 = 0.0; // De initiele intergral error
MBroek 10:755b9228cc42 136
MBroek 13:b0d3c547bf2f 137 double error_prev_2 = 0.0;
MBroek 13:b0d3c547bf2f 138 double error_int_2 = 0.0;
MBroek 5:c510ab61af28 139
MBroek 20:2fdb069ffcae 140 double reference_pos_m2; // De reference positie waar de motor heen wil afhankelijk van het EMG signaal
MBroek 20:2fdb069ffcae 141 double reference_pos_m1;
MBroek 20:2fdb069ffcae 142
MBroek 17:525b785f007a 143 enum which_motor{motor1, motor2}; // enum van de motoren
MBroek 15:3f3d87513a6b 144 which_motor motor_that_runs = motor1;
MBroek 15:3f3d87513a6b 145
MBroek 24:ea6de9237cff 146 int digital0; // Define digital signals fot EMG
MBroek 24:ea6de9237cff 147 int digital1;
MBroek 24:ea6de9237cff 148
MBroek 24:ea6de9237cff 149 double threshold0 = 0.3; // Define thresholds (signal is normalized, so 0.3 = 30% of maximum value) fot EMG
MBroek 24:ea6de9237cff 150 double threshold1 = 0.3;
MBroek 24:ea6de9237cff 151
MBroek 24:ea6de9237cff 152 double max_out0 = 0; // Set initial maximum values of the signals to 0, the code will check if there's a new maximum value every iteration
MBroek 24:ea6de9237cff 153 double max_out1 = 0;
MBroek 24:ea6de9237cff 154
MBroek 9:f735baee0c2b 155
MBroek 0:9e053ed05c69 156
MBroek 0:9e053ed05c69 157
MBroek 0:9e053ed05c69 158
MBroek 0:9e053ed05c69 159
MBroek 0:9e053ed05c69 160 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 0:9e053ed05c69 161
MBroek 13:b0d3c547bf2f 162
MBroek 15:3f3d87513a6b 163 // De statechanger -----------------------------------------------------------
MBroek 15:3f3d87513a6b 164 void state_changer(){
MBroek 15:3f3d87513a6b 165 if(states == off){
MBroek 21:3db3f2d56d56 166 states = init;
MBroek 16:5a749b319276 167 }
MBroek 21:3db3f2d56d56 168 else if(states == init){
MBroek 28:97b9e50c1180 169 states = init_fish;
MBroek 28:97b9e50c1180 170 }
MBroek 28:97b9e50c1180 171 else if(states == init_fish){
MBroek 16:5a749b319276 172 states = run;
MBroek 16:5a749b319276 173 }
MBroek 16:5a749b319276 174 else{
MBroek 16:5a749b319276 175 states = off;
MBroek 16:5a749b319276 176 }
MBroek 16:5a749b319276 177 }
MBroek 16:5a749b319276 178
MBroek 13:b0d3c547bf2f 179
MBroek 9:f735baee0c2b 180 // Functie voor het switchen tussen de motors ------------------------------
MBroek 9:f735baee0c2b 181 void motor_switch(){
MBroek 15:3f3d87513a6b 182 if(motor_that_runs == motor1){
MBroek 15:3f3d87513a6b 183 motor_that_runs = motor2;
MBroek 15:3f3d87513a6b 184 }
MBroek 15:3f3d87513a6b 185 else{
MBroek 15:3f3d87513a6b 186 motor_that_runs = motor1;
MBroek 15:3f3d87513a6b 187 }
MBroek 16:5a749b319276 188 }
MBroek 15:3f3d87513a6b 189
MBroek 9:f735baee0c2b 190
MBroek 1:5b3fa8e47e8b 191 // Functie voor het vinden van de positie van motor 1 ---------------------
MBroek 13:b0d3c547bf2f 192 double get_position_m1(const double radpercount){ //returned de positie van de motor in radialen (positie vd arm in radialen)
MBroek 0:9e053ed05c69 193 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 21:3db3f2d56d56 194 return radpercount * counts1 + initial_pos_m1; // rekent positie motor1 uit en geeft deze als output vd functie
MBroek 1:5b3fa8e47e8b 195 }
MBroek 1:5b3fa8e47e8b 196
MBroek 1:5b3fa8e47e8b 197
MBroek 1:5b3fa8e47e8b 198 // Functie voor vinden van de positie van motor 2 -----------------------------
MBroek 23:3902ee714281 199 double get_position_m2(const double meterpercount, double mot_pos_prev){ // returned de positie van het karretje in meter
MBroek 23:3902ee714281 200 double mot_pos;
MBroek 1:5b3fa8e47e8b 201 counts2 = encoder_motor2.getPulses(); // leest aantal pulses vd encoder af
MBroek 23:3902ee714281 202 mot_pos = meterpercount * counts2 + mot_pos_prev;
MBroek 23:3902ee714281 203 encoder_motor2.reset();
MBroek 23:3902ee714281 204 mot_pos_prev = mot_pos;
MBroek 23:3902ee714281 205 return mot_pos_prev; // rekent de positie van het karretje uit en geeft dit als output vd functie
MBroek 1:5b3fa8e47e8b 206 }
MBroek 1:5b3fa8e47e8b 207
MBroek 1:5b3fa8e47e8b 208
MBroek 24:ea6de9237cff 209 // Functie voor het uitlezen van het EMG signaal ------------------------------------------
MBroek 24:ea6de9237cff 210 int get_EMG()
MBroek 24:ea6de9237cff 211 {
MBroek 24:ea6de9237cff 212 double in0 = emg0.read(); // EMG signal 0 (right biceps)
MBroek 24:ea6de9237cff 213 double in1 = emg1.read(); // EMG signal 1 (left biceps)
MBroek 24:ea6de9237cff 214
MBroek 24:ea6de9237cff 215 double filtered0 = bqc1.step(in0); // Filter signals (high pass and notch)
MBroek 24:ea6de9237cff 216 double filtered1 = bqc2.step(in1);
MBroek 24:ea6de9237cff 217 double abs_filtered0 = abs(filtered0); // Absolute value of filtered signals
MBroek 24:ea6de9237cff 218 double abs_filtered1 = abs(filtered1);
MBroek 24:ea6de9237cff 219 double out0 = bqc3.step(abs_filtered0); // Filter the signals again to remove trend (low pass)
MBroek 24:ea6de9237cff 220 double out1 = bqc4.step(abs_filtered1);
MBroek 24:ea6de9237cff 221
MBroek 24:ea6de9237cff 222 if (out0 > max_out0) // Check if the signals have a new maximum value
MBroek 24:ea6de9237cff 223 { max_out0 = out0; }
MBroek 24:ea6de9237cff 224 if (out1 > max_out1)
MBroek 24:ea6de9237cff 225 { max_out1 = out1; }
MBroek 24:ea6de9237cff 226
MBroek 24:ea6de9237cff 227 double normalized_out0 = out0 / max_out0; // Normalize the signals (divide by maximum value to scale from 0 to 1)
MBroek 24:ea6de9237cff 228 double normalized_out1 = out1 / max_out1;
MBroek 24:ea6de9237cff 229
MBroek 24:ea6de9237cff 230 if (normalized_out0 > threshold0) // If the signal value is above the threshold, give an output of 1
MBroek 24:ea6de9237cff 231 { digital0 = 1; }
MBroek 24:ea6de9237cff 232 else // If the signal value is below the threshold, give an output of 0
MBroek 24:ea6de9237cff 233 { digital0 = 0; }
MBroek 24:ea6de9237cff 234
MBroek 24:ea6de9237cff 235 if (normalized_out1 > threshold1) // Do the same for the second EMG signal
MBroek 24:ea6de9237cff 236 { digital1 = 1; }
MBroek 24:ea6de9237cff 237 else
MBroek 24:ea6de9237cff 238 { digital1 = 0; }
MBroek 24:ea6de9237cff 239
MBroek 28:97b9e50c1180 240 /*
MBroek 24:ea6de9237cff 241 scope.set(0,in0); // EMG signal 0 (right biceps)
MBroek 24:ea6de9237cff 242 scope.set(1,in1); // EMG signal 1 (left biceps)
MBroek 24:ea6de9237cff 243 scope.set(2,digital0); // Signal 0 after filtering, detrending, rectification, digitalization
MBroek 24:ea6de9237cff 244 scope.set(3,digital1); // Signal 1 after filtering, detrending, rectification, digitalization
MBroek 24:ea6de9237cff 245 scope.set(4,digital0 - digital1); // Final output signal
MBroek 24:ea6de9237cff 246
MBroek 24:ea6de9237cff 247 scope.send(); // Send all channels to the PC at once
MBroek 28:97b9e50c1180 248 */
MBroek 24:ea6de9237cff 249
MBroek 24:ea6de9237cff 250 return digital0 - digital1; // Subtract the digital signals (right arm gives positive values, left arm give negative values)
MBroek 24:ea6de9237cff 251 }
MBroek 24:ea6de9237cff 252
MBroek 24:ea6de9237cff 253
MBroek 24:ea6de9237cff 254
MBroek 5:c510ab61af28 255 // Functie voor het vinden van de reference position van motor 1 --------------------
MBroek 21:3db3f2d56d56 256 double get_reference_position_m1(double &ref_pos_prev, const double vrijheid_rad){
MBroek 22:6dfe5554b96e 257 double ref_pos;
MBroek 28:97b9e50c1180 258 // int final_signal = get_EMG(); // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
MBroek 28:97b9e50c1180 259 int final_signal = EMG_sim1.read() - EMG_sim2.read();
MBroek 21:3db3f2d56d56 260 switch(final_signal){
MBroek 21:3db3f2d56d56 261 case 0 :
MBroek 22:6dfe5554b96e 262 ref_pos = ref_pos_prev;
MBroek 21:3db3f2d56d56 263 break;
MBroek 21:3db3f2d56d56 264 case 1 :
MBroek 30:457e42514c47 265 ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad*0.3; // Dit dwingt af dat de ref pos veranderd met ong 0.5 rad/s
MBroek 21:3db3f2d56d56 266 break;
MBroek 21:3db3f2d56d56 267 case -1 :
MBroek 30:457e42514c47 268 ref_pos = ref_pos_prev - T_motor_function*vrijheid_rad*0.3;
MBroek 21:3db3f2d56d56 269 break;
MBroek 21:3db3f2d56d56 270 }
MBroek 22:6dfe5554b96e 271 if(ref_pos >= vrijheid_rad){
MBroek 22:6dfe5554b96e 272 ref_pos = vrijheid_rad;
MBroek 21:3db3f2d56d56 273 }
MBroek 22:6dfe5554b96e 274 if(ref_pos <= -vrijheid_rad){
MBroek 22:6dfe5554b96e 275 ref_pos = -vrijheid_rad;
MBroek 21:3db3f2d56d56 276 }
MBroek 22:6dfe5554b96e 277 ref_pos_prev = ref_pos;
MBroek 21:3db3f2d56d56 278 return ref_pos_prev; // vrijheid_rad is de uiterste positie vd arm in radialen
MBroek 5:c510ab61af28 279 }
MBroek 5:c510ab61af28 280
MBroek 5:c510ab61af28 281
MBroek 5:c510ab61af28 282 // Functie voor het vinden van de reference position van motor 2 --------------------
MBroek 23:3902ee714281 283 double get_reference_position_m2(double &ref_pos_prev, const double vrijheid_meter){
MBroek 23:3902ee714281 284 double ref_pos;
MBroek 23:3902ee714281 285 int final_signal = EMG_sim1.read() - EMG_sim2.read(); // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
MBroek 23:3902ee714281 286 switch(final_signal){
MBroek 23:3902ee714281 287 case 0 :
MBroek 23:3902ee714281 288 ref_pos = ref_pos_prev;
MBroek 23:3902ee714281 289 break;
MBroek 23:3902ee714281 290 case 1 :
MBroek 23:3902ee714281 291 ref_pos = ref_pos_prev + T_motor_function*v_max_karretje; // Hierdoor veranderd de ref_pos met de maximale snelheid vd motor (karretje)
MBroek 23:3902ee714281 292 break;
MBroek 23:3902ee714281 293 case -1 :
MBroek 23:3902ee714281 294 ref_pos = ref_pos_prev - T_motor_function*v_max_karretje;
MBroek 23:3902ee714281 295 break;
MBroek 23:3902ee714281 296 }
MBroek 23:3902ee714281 297 if(ref_pos >= vrijheid_meter){
MBroek 23:3902ee714281 298 ref_pos = vrijheid_meter;
MBroek 23:3902ee714281 299 }
MBroek 23:3902ee714281 300 if(ref_pos <= 0){
MBroek 23:3902ee714281 301 ref_pos = 0;
MBroek 23:3902ee714281 302 }
MBroek 23:3902ee714281 303 ref_pos_prev = ref_pos;
MBroek 23:3902ee714281 304 return ref_pos_prev;
MBroek 1:5b3fa8e47e8b 305 }
MBroek 23:3902ee714281 306
MBroek 3:581c5918b590 307
MBroek 3:581c5918b590 308 // HIDScope functie ----------------------------------------------------------------------
MBroek 7:435f984781ab 309 void set_scope(double input1, double input2, double input3){
MBroek 7:435f984781ab 310 scope.set(0, input1);
MBroek 7:435f984781ab 311 scope.set(1, input2);
MBroek 7:435f984781ab 312 scope.set(2, input3);
MBroek 3:581c5918b590 313 scope.send();
MBroek 3:581c5918b590 314 }
MBroek 3:581c5918b590 315
MBroek 0:9e053ed05c69 316
MBroek 1:5b3fa8e47e8b 317 // De PID-controller voor de motors -------------------------------------------------
MBroek 6:e0857842e8cd 318 double PID_controller(double ref_pos, double mot_pos, double &e_prev, double &e_int, const double Kp, const double Kd, const double Ki){
MBroek 0:9e053ed05c69 319 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 21:3db3f2d56d56 320 double error_dif = (error-e_prev)/T_motor_function; // De error differentieren
MBroek 29:2b711fc9a5b2 321 //scope.set(error_dif,0);
MBroek 29:2b711fc9a5b2 322 //error_dif = bqc5.step(error_dif); // Filter biquad
MBroek 29:2b711fc9a5b2 323 //scope.set(error_dif,1); scope.send();
MBroek 6:e0857842e8cd 324 e_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 21:3db3f2d56d56 325 e_int = e_int + T_motor_function*error; // De error integreren
MBroek 6:e0857842e8cd 326 return Kp*error + Ki*e_int + Kd*error_dif; // De uiteindelijke PID output
MBroek 0:9e053ed05c69 327 }
MBroek 0:9e053ed05c69 328
MBroek 0:9e053ed05c69 329
MBroek 2:6bef5397e8a9 330 // Motor 1 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 331 void set_motor1(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 332 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 333 motor1_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 334 }
MBroek 2:6bef5397e8a9 335 else {
MBroek 2:6bef5397e8a9 336 motor1_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 337 }
MBroek 2:6bef5397e8a9 338 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 339 motor_input = 1;
MBroek 2:6bef5397e8a9 340 }
MBroek 2:6bef5397e8a9 341 motor1_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 342 }
MBroek 2:6bef5397e8a9 343
MBroek 2:6bef5397e8a9 344
MBroek 2:6bef5397e8a9 345 // Motor 2 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 346 void set_motor2(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 347 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 348 motor2_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 349 }
MBroek 2:6bef5397e8a9 350 else {
MBroek 2:6bef5397e8a9 351 motor2_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 352 }
MBroek 2:6bef5397e8a9 353 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 354 motor_input = 1;
MBroek 2:6bef5397e8a9 355 }
MBroek 2:6bef5397e8a9 356 motor2_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 357 }
MBroek 2:6bef5397e8a9 358
MBroek 0:9e053ed05c69 359
MBroek 4:6524b198721f 360 // De go-flag functies -----------------------------------------------------------------
MBroek 4:6524b198721f 361 void flag1_activate(){flag1=true;} // Activeert de flag
MBroek 14:f51d51395803 362 void flag2_activate(){flag2=true;}
MBroek 14:f51d51395803 363
MBroek 14:f51d51395803 364
MBroek 21:3db3f2d56d56 365 // Dit doet de robot als hij niets doet ------------------------------------------------------
MBroek 21:3db3f2d56d56 366 void robot_is_off(){
MBroek 21:3db3f2d56d56 367 ledgreen.write(1);
MBroek 21:3db3f2d56d56 368 ledred.write(0); // Indicator ik ben uit
MBroek 21:3db3f2d56d56 369 motor1_speed_pin = 0; // Uitzetten vd motoren
MBroek 16:5a749b319276 370 motor2_speed_pin = 0;
MBroek 16:5a749b319276 371 }
MBroek 16:5a749b319276 372
MBroek 16:5a749b319276 373
MBroek 28:97b9e50c1180 374 // De initialisatie functie --------------------------------------------------------------------
MBroek 28:97b9e50c1180 375 void initialise(){
MBroek 28:97b9e50c1180 376 ledred.write(1);
MBroek 28:97b9e50c1180 377 ledblue.write(0);
MBroek 28:97b9e50c1180 378 if (flag1){
MBroek 28:97b9e50c1180 379 double init_m2_speed = init_pot;
MBroek 28:97b9e50c1180 380 set_motor2(init_m2_speed);
MBroek 28:97b9e50c1180 381 }
MBroek 28:97b9e50c1180 382 }
MBroek 28:97b9e50c1180 383
MBroek 28:97b9e50c1180 384
MBroek 28:97b9e50c1180 385 // De initialiseren beeindigen functie ------------------------------------------------------------
MBroek 28:97b9e50c1180 386 void initialise_finish(){ // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
MBroek 28:97b9e50c1180 387 ledblue.write(1);
MBroek 21:3db3f2d56d56 388 ref_pos_prev_m1 = initial_pos_m1;
MBroek 21:3db3f2d56d56 389 ref_pos_prev_m2 = initial_pos_m2;
MBroek 28:97b9e50c1180 390 position_motor1 = initial_pos_m1;
MBroek 28:97b9e50c1180 391 position_motor2 = initial_pos_m2;
MBroek 28:97b9e50c1180 392 position_motor2_prev = initial_pos_m2;
MBroek 30:457e42514c47 393 PID_output_1 = 0.0;
MBroek 30:457e42514c47 394 PID_output_2 = 0.0;
MBroek 16:5a749b319276 395 ledgreen.write(0);
MBroek 28:97b9e50c1180 396 ledred.write(0);
MBroek 21:3db3f2d56d56 397 encoder_motor1.reset();
MBroek 21:3db3f2d56d56 398 encoder_motor2.reset();
MBroek 16:5a749b319276 399 motor1_speed_pin = 0;
MBroek 21:3db3f2d56d56 400 }
MBroek 16:5a749b319276 401
MBroek 16:5a749b319276 402
MBroek 15:3f3d87513a6b 403 // De functie die de motoren aanstuurt -------------------------------------------------
MBroek 17:525b785f007a 404 void running_motors(){ // Deze functie kijkt welke motor moet draaien en rekent dan de PID uit en geeft dit door aan de motor input functie
MBroek 15:3f3d87513a6b 405 if (flag1){
MBroek 15:3f3d87513a6b 406 flag1 = false;
MBroek 16:5a749b319276 407 ledred.write(1);
MBroek 15:3f3d87513a6b 408 switch (motor_that_runs){
MBroek 15:3f3d87513a6b 409 case motor1 : // In deze case draait alleen motor 1
MBroek 21:3db3f2d56d56 410 reference_pos_m1 = get_reference_position_m1(ref_pos_prev_m1, vrijheid_m1_rad);
MBroek 28:97b9e50c1180 411 position_motor1 = get_position_m1(rad_per_count);
MBroek 28:97b9e50c1180 412 PID_output_1 = PID_controller(reference_pos_m1, position_motor1, error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
MBroek 28:97b9e50c1180 413 //PID_output_2 = PID_controller(-reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt er voor dat motor2 meedraait met motor1
MBroek 15:3f3d87513a6b 414 set_motor1(PID_output_1);
MBroek 28:97b9e50c1180 415 //set_motor2(-PID_output_1);
MBroek 23:3902ee714281 416 encoder_motor2.reset();
MBroek 15:3f3d87513a6b 417 break;
MBroek 15:3f3d87513a6b 418 case motor2 :
MBroek 23:3902ee714281 419 reference_pos_m2 = get_reference_position_m2(ref_pos_prev_m2, vrijheid_m2_meter);
MBroek 23:3902ee714281 420 position_motor2 = get_position_m2(meter_per_count, position_motor2_prev);
MBroek 23:3902ee714281 421 PID_output_2 = PID_controller(reference_pos_m2, position_motor2, error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
MBroek 20:2fdb069ffcae 422 PID_output_1 = PID_controller(reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt ervoor dat motor1 op de dezelfde positie blijft als motor 2 draait.
MBroek 15:3f3d87513a6b 423 set_motor2(PID_output_2);
MBroek 20:2fdb069ffcae 424 set_motor1(PID_output_1);
MBroek 23:3902ee714281 425 position_motor2_prev = position_motor2;
MBroek 15:3f3d87513a6b 426 break;
MBroek 15:3f3d87513a6b 427 }
MBroek 15:3f3d87513a6b 428 }
MBroek 15:3f3d87513a6b 429 }
MBroek 15:3f3d87513a6b 430
MBroek 15:3f3d87513a6b 431
MBroek 15:3f3d87513a6b 432 // De HIDscope debug functie ----------------------------------------------------------------
MBroek 17:525b785f007a 433 void call_HIDscope(double input_1, double input_2, double input_3){ // Deze functie roept de HIDscope aan
MBroek 15:3f3d87513a6b 434 if (flag2){
MBroek 15:3f3d87513a6b 435 flag2 = false;
MBroek 15:3f3d87513a6b 436 set_scope(input_1, input_2, input_3);
MBroek 15:3f3d87513a6b 437 }
MBroek 15:3f3d87513a6b 438 }
MBroek 15:3f3d87513a6b 439
MBroek 15:3f3d87513a6b 440
MBroek 15:3f3d87513a6b 441
MBroek 0:9e053ed05c69 442
MBroek 0:9e053ed05c69 443 // DE MAIN =================================================================================================================
MBroek 0:9e053ed05c69 444 int main()
MBroek 0:9e053ed05c69 445 {
MBroek 0:9e053ed05c69 446 pc.baud(SERIALBAUD);
MBroek 0:9e053ed05c69 447
MBroek 28:97b9e50c1180 448 bqc1.add( &bq1 ).add( &bq2 ).add( &bq3 );
MBroek 28:97b9e50c1180 449 bqc2.add( &bq4 ).add( &bq5 ).add( &bq6 );
MBroek 28:97b9e50c1180 450 bqc3.add( &bq7 ).add( &bq8 );
MBroek 28:97b9e50c1180 451 bqc4.add( &bq9 ).add( &bq10 );
MBroek 28:97b9e50c1180 452 bqc5.add( &bq11 ).add( &bq12 );
MBroek 28:97b9e50c1180 453
MBroek 28:97b9e50c1180 454
MBroek 28:97b9e50c1180 455
MBroek 28:97b9e50c1180 456
MBroek 16:5a749b319276 457 state_switch_button.fall(&state_changer); // Switcht states
MBroek 17:525b785f007a 458 motor_switch_button.fall(&motor_switch); // Switcht motors
MBroek 16:5a749b319276 459
MBroek 17:525b785f007a 460
MBroek 13:b0d3c547bf2f 461
MBroek 21:3db3f2d56d56 462 motor_ticker.attach(&flag1_activate, T_motor_function); // Activeert de go-flag van motor positie
MBroek 17:525b785f007a 463 hidscope_ticker.attach(&flag2_activate,0.01); // Leest ref pos en mot pos uit, rekent PID uit en stuurt motoren aan.
MBroek 13:b0d3c547bf2f 464
MBroek 16:5a749b319276 465 while(1){
MBroek 14:f51d51395803 466 switch(states){
MBroek 16:5a749b319276 467 case off : // Dan staat het programma stil en gebeurt er niks
MBroek 16:5a749b319276 468 robot_is_off();
MBroek 14:f51d51395803 469 break;
MBroek 21:3db3f2d56d56 470 case init : // Hier voert hij alleen het initialiseren van motor 1 uit
MBroek 21:3db3f2d56d56 471 initialise();
MBroek 16:5a749b319276 472 break;
MBroek 28:97b9e50c1180 473 case init_fish :
MBroek 28:97b9e50c1180 474 initialise_finish();
MBroek 28:97b9e50c1180 475 break;
MBroek 15:3f3d87513a6b 476 case run :
MBroek 17:525b785f007a 477 running_motors(); // Hier voert hij het programma voor het aansturen vd motors uit
MBroek 15:3f3d87513a6b 478 break;
MBroek 14:f51d51395803 479
MBroek 15:3f3d87513a6b 480 }
MBroek 28:97b9e50c1180 481 call_HIDscope(PID_output_1, reference_pos_m1, position_motor1);
MBroek 15:3f3d87513a6b 482 }
MBroek 15:3f3d87513a6b 483 }
MBroek 14:f51d51395803 484
MBroek 14:f51d51395803 485
MBroek 14:f51d51395803 486
MBroek 14:f51d51395803 487
MBroek 14:f51d51395803 488
MBroek 14:f51d51395803 489
MBroek 14:f51d51395803 490
MBroek 14:f51d51395803 491
MBroek 0:9e053ed05c69 492
MBroek 0:9e053ed05c69 493
MBroek 0:9e053ed05c69 494
MBroek 0:9e053ed05c69 495
MBroek 0:9e053ed05c69 496
MBroek 0:9e053ed05c69 497
MBroek 0:9e053ed05c69 498
MBroek 0:9e053ed05c69 499
MBroek 0:9e053ed05c69 500
MBroek 0:9e053ed05c69 501
MBroek 0:9e053ed05c69 502
MBroek 0:9e053ed05c69 503
MBroek 0:9e053ed05c69 504
MBroek 0:9e053ed05c69 505
MBroek 0:9e053ed05c69 506
MBroek 0:9e053ed05c69 507
MBroek 0:9e053ed05c69 508
MBroek 0:9e053ed05c69 509
MBroek 0:9e053ed05c69 510
MBroek 0:9e053ed05c69 511
MBroek 0:9e053ed05c69 512
MBroek 0:9e053ed05c69 513
MBroek 0:9e053ed05c69 514
MBroek 0:9e053ed05c69 515
MBroek 0:9e053ed05c69 516
MBroek 0:9e053ed05c69 517
MBroek 0:9e053ed05c69 518
MBroek 0:9e053ed05c69 519
MBroek 0:9e053ed05c69 520
MBroek 0:9e053ed05c69 521
MBroek 0:9e053ed05c69 522
MBroek 0:9e053ed05c69 523
MBroek 0:9e053ed05c69 524
MBroek 0:9e053ed05c69 525
MBroek 0:9e053ed05c69 526
MBroek 0:9e053ed05c69 527
MBroek 0:9e053ed05c69 528
MBroek 0:9e053ed05c69 529
MBroek 0:9e053ed05c69 530
MBroek 0:9e053ed05c69 531
MBroek 0:9e053ed05c69 532
MBroek 0:9e053ed05c69 533
MBroek 0:9e053ed05c69 534
MBroek 0:9e053ed05c69 535
MBroek 0:9e053ed05c69 536
MBroek 0:9e053ed05c69 537
MBroek 0:9e053ed05c69 538
MBroek 0:9e053ed05c69 539
MBroek 0:9e053ed05c69 540
MBroek 0:9e053ed05c69 541
MBroek 0:9e053ed05c69 542
MBroek 0:9e053ed05c69 543
MBroek 0:9e053ed05c69 544
MBroek 0:9e053ed05c69 545