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@9:f735baee0c2b, 2016-10-17 (annotated)

Committer:
MBroek
Date:
Mon Oct 17 09:37:38 2016 +0000
Revision:
9:f735baee0c2b
Parent:
8:919ddba2875e
Child:
10:755b9228cc42
Motor 2 test, doet nog raar

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 3:581c5918b590 11
MBroek 3:581c5918b590 12 // Definieren van de HIDscope ----------------------------------------
MBroek 7:435f984781ab 13 HIDScope scope(3);
MBroek 0:9e053ed05c69 14
MBroek 0:9e053ed05c69 15
MBroek 0:9e053ed05c69 16 // Definieren van de Serial en Encoder -------------------------------
MBroek 0:9e053ed05c69 17 MODSERIAL pc(USBTX, USBRX);
MBroek 0:9e053ed05c69 18 #define SERIALBAUD 115200
MBroek 0:9e053ed05c69 19
MBroek 0:9e053ed05c69 20 QEI encoder_motor1(D10,D11,NC,64);
MBroek 0:9e053ed05c69 21 QEI encoder_motor2(D12,D13,NC,64);
MBroek 0:9e053ed05c69 22
MBroek 0:9e053ed05c69 23
MBroek 0:9e053ed05c69 24 // Definieren van de Motorpins ---------------------------------------
MBroek 0:9e053ed05c69 25 DigitalOut motor1_direction_pin(D7);
MBroek 0:9e053ed05c69 26 PwmOut motor1_speed_pin(D6);
MBroek 0:9e053ed05c69 27
MBroek 0:9e053ed05c69 28 DigitalOut motor2_direction_pin(D4);
MBroek 0:9e053ed05c69 29 PwmOut motor2_speed_pin(D5);
MBroek 0:9e053ed05c69 30
MBroek 0:9e053ed05c69 31
MBroek 0:9e053ed05c69 32 //Definieren van bord-elementen --------------------------------------
MBroek 3:581c5918b590 33 InterruptIn kill_switch(D8);
MBroek 3:581c5918b590 34 InterruptIn test_button(D9);
MBroek 1:5b3fa8e47e8b 35 AnalogIn pot1(A1); // Dit is de gesimuleerde emg1
MBroek 1:5b3fa8e47e8b 36 AnalogIn pot2(A2); // Dit is de gesimuleerde emg2
MBroek 0:9e053ed05c69 37
MBroek 0:9e053ed05c69 38
MBroek 4:6524b198721f 39 //Definieren van de tickers ------------------------------------------
MBroek 4:6524b198721f 40 Ticker test_ticker;
MBroek 4:6524b198721f 41 Ticker hidscope_ticker;
MBroek 4:6524b198721f 42
MBroek 4:6524b198721f 43
MBroek 0:9e053ed05c69 44
MBroek 0:9e053ed05c69 45 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
MBroek 4:6524b198721f 46
MBroek 0:9e053ed05c69 47 volatile bool safe = true; // Conditie voor de while-loop in main
MBroek 0:9e053ed05c69 48
MBroek 4:6524b198721f 49 double position_motor1;
MBroek 4:6524b198721f 50
MBroek 4:6524b198721f 51 int counts1; // Pulses van motoren
MBroek 4:6524b198721f 52 int counts2;
MBroek 0:9e053ed05c69 53
MBroek 0:9e053ed05c69 54 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 4:6524b198721f 55 const double rad_per_count = pi/8400.0; // Aantal rad per puls uit encoder
MBroek 0:9e053ed05c69 56
MBroek 9:f735baee0c2b 57 const double radius_tandwiel = 0.008;
MBroek 9:f735baee0c2b 58 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 59
MBroek 4:6524b198721f 60 double error1_int = 0.00000; // Initiele error integral waardes
MBroek 4:6524b198721f 61 double error2_int = 0.00000;
MBroek 0:9e053ed05c69 62
MBroek 0:9e053ed05c69 63 const double T_getpos = 0.01; // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
MBroek 0:9e053ed05c69 64
MBroek 4:6524b198721f 65 volatile bool flag1 = false, flag2 = false; // De flags voor de functies aangeroepen door de tickers
MBroek 4:6524b198721f 66
MBroek 6:e0857842e8cd 67 const double Kp_1 = 1.0000000; // De PID variablen voor motor 1
MBroek 8:919ddba2875e 68 const double Kd_1 = 0.1;
MBroek 8:919ddba2875e 69 const double Ki_1 = 0.3;
MBroek 4:6524b198721f 70
MBroek 9:f735baee0c2b 71 const double Kp_2 = 1.0000000; // De PID variablen voor motor 2
MBroek 9:f735baee0c2b 72 const double Kd_2 = 0.1;
MBroek 9:f735baee0c2b 73 const double Ki_2 = 0.3;
MBroek 5:c510ab61af28 74
MBroek 9:f735baee0c2b 75 const double vrijheid_m1_rad = 0.5*pi; // Dit is de uiterste postitie vd arm in radialen
MBroek 9:f735baee0c2b 76 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 77
MBroek 9:f735baee0c2b 78 double reference_position_motor1 = 0.5*pi; // Dit zijn de eerste posities waar de motoren heen willen, deze moeten dezelfde zijn als de startpositie!
MBroek 9:f735baee0c2b 79 double reference_position_motor3 = 0;
MBroek 9:f735baee0c2b 80
MBroek 9:f735baee0c2b 81 double PID_output_1 = 0; // De eerste PID_output, deze is nul anders gaan de motoren draaien
MBroek 9:f735baee0c2b 82 double PID_output_2 = 0;
MBroek 5:c510ab61af28 83
MBroek 6:e0857842e8cd 84 double error_prev = 0; // De initiele previous error
MBroek 6:e0857842e8cd 85 double error_int = 0; // De initiele intergral error
MBroek 5:c510ab61af28 86
MBroek 9:f735baee0c2b 87 bool which_motor = 0;
MBroek 9:f735baee0c2b 88
MBroek 0:9e053ed05c69 89
MBroek 0:9e053ed05c69 90
MBroek 0:9e053ed05c69 91
MBroek 0:9e053ed05c69 92
MBroek 0:9e053ed05c69 93 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 0:9e053ed05c69 94
MBroek 0:9e053ed05c69 95 // De emergency break ------------------------------------------------
MBroek 0:9e053ed05c69 96 void emergency_stop(){
MBroek 0:9e053ed05c69 97 safe = false;
MBroek 0:9e053ed05c69 98 pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
MBroek 0:9e053ed05c69 99 }
MBroek 0:9e053ed05c69 100
MBroek 1:5b3fa8e47e8b 101
MBroek 9:f735baee0c2b 102 // Functie voor het switchen tussen de motors ------------------------------
MBroek 9:f735baee0c2b 103 void motor_switch(){
MBroek 9:f735baee0c2b 104 which_motor = !which_motor; // =0 activeert motor1 en =1 activeert motor2
MBroek 9:f735baee0c2b 105 }
MBroek 9:f735baee0c2b 106
MBroek 9:f735baee0c2b 107
MBroek 1:5b3fa8e47e8b 108 // Functie voor het vinden van de positie van motor 1 ---------------------
MBroek 1:5b3fa8e47e8b 109 double get_position_m1(const double radpercount){ //returned de positie van de motor in radialen (positie vd arm in radialen)
MBroek 0:9e053ed05c69 110 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 1:5b3fa8e47e8b 111 return radpercount * counts1; // rekent positie motor1 uit en geeft deze als output vd functie
MBroek 1:5b3fa8e47e8b 112 }
MBroek 1:5b3fa8e47e8b 113
MBroek 1:5b3fa8e47e8b 114
MBroek 1:5b3fa8e47e8b 115 // Functie voor vinden van de positie van motor 2 -----------------------------
MBroek 1:5b3fa8e47e8b 116 double get_position_m2(const double meterpercount){ // returned de positie van het karretje in meter
MBroek 1:5b3fa8e47e8b 117 counts2 = encoder_motor2.getPulses(); // leest aantal pulses vd encoder af
MBroek 1:5b3fa8e47e8b 118 return meterpercount * counts2; // rekent de positie van het karretje uit en geeft dit als output vd functie
MBroek 1:5b3fa8e47e8b 119 }
MBroek 1:5b3fa8e47e8b 120
MBroek 1:5b3fa8e47e8b 121
MBroek 5:c510ab61af28 122 // Functie voor het vinden van de reference position van motor 1 --------------------
MBroek 5:c510ab61af28 123 double get_reference_position_m1(const double aantal_rad){
MBroek 4:6524b198721f 124 double ref_pos = pot1.read() - pot2.read(); // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
MBroek 5:c510ab61af28 125 return ref_pos * aantal_rad; // Aantal rad is de uiterste positie vd arm in radialen
MBroek 5:c510ab61af28 126 }
MBroek 5:c510ab61af28 127
MBroek 5:c510ab61af28 128
MBroek 5:c510ab61af28 129 // Functie voor het vinden van de reference position van motor 2 --------------------
MBroek 5:c510ab61af28 130 double get_reference_position_m2(const double aantal_meter){
MBroek 5:c510ab61af28 131 double ref_pos = pot1.read() - pot2.read(); // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
MBroek 5:c510ab61af28 132 return ref_pos * aantal_meter; // Aantal meter is hoeveelheid radialen van middelpunt tot minima.
MBroek 1:5b3fa8e47e8b 133 }
MBroek 0:9e053ed05c69 134
MBroek 3:581c5918b590 135
MBroek 3:581c5918b590 136 // HIDScope functie ----------------------------------------------------------------------
MBroek 7:435f984781ab 137 void set_scope(double input1, double input2, double input3){
MBroek 7:435f984781ab 138 scope.set(0, input1);
MBroek 7:435f984781ab 139 scope.set(1, input2);
MBroek 7:435f984781ab 140 scope.set(2, input3);
MBroek 3:581c5918b590 141 scope.send();
MBroek 3:581c5918b590 142 }
MBroek 3:581c5918b590 143
MBroek 0:9e053ed05c69 144
MBroek 1:5b3fa8e47e8b 145 // De PID-controller voor de motors -------------------------------------------------
MBroek 6:e0857842e8cd 146 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 147 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 6:e0857842e8cd 148 double error_dif = (error-e_prev)/T_getpos; // De error differentieren
MBroek 1:5b3fa8e47e8b 149 //error_dif = ..... // Filter biquad poep
MBroek 6:e0857842e8cd 150 e_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 6:e0857842e8cd 151 e_int = e_int + T_getpos*error; // De error integreren
MBroek 6:e0857842e8cd 152 return Kp*error + Ki*e_int + Kd*error_dif; // De uiteindelijke PID output
MBroek 0:9e053ed05c69 153 }
MBroek 0:9e053ed05c69 154
MBroek 0:9e053ed05c69 155
MBroek 2:6bef5397e8a9 156 // Motor 1 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 157 void set_motor1(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 158 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 159 motor1_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 160 }
MBroek 2:6bef5397e8a9 161 else {
MBroek 2:6bef5397e8a9 162 motor1_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 163 }
MBroek 2:6bef5397e8a9 164 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 165 motor_input = 1;
MBroek 2:6bef5397e8a9 166 }
MBroek 2:6bef5397e8a9 167 motor1_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 168 }
MBroek 2:6bef5397e8a9 169
MBroek 2:6bef5397e8a9 170
MBroek 2:6bef5397e8a9 171 // Motor 2 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 172 void set_motor2(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 173 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 174 motor2_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 175 }
MBroek 2:6bef5397e8a9 176 else {
MBroek 2:6bef5397e8a9 177 motor2_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 178 }
MBroek 2:6bef5397e8a9 179 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 180 motor_input = 1;
MBroek 2:6bef5397e8a9 181 }
MBroek 2:6bef5397e8a9 182 motor2_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 183 }
MBroek 2:6bef5397e8a9 184
MBroek 0:9e053ed05c69 185
MBroek 4:6524b198721f 186 // De go-flag functies -----------------------------------------------------------------
MBroek 4:6524b198721f 187 void flag1_activate(){flag1=true;} // Activeert de flag
MBroek 4:6524b198721f 188 void flag2_activate(){flag2=true;}
MBroek 4:6524b198721f 189
MBroek 4:6524b198721f 190
MBroek 0:9e053ed05c69 191
MBroek 0:9e053ed05c69 192 // DE MAIN =================================================================================================================
MBroek 0:9e053ed05c69 193 int main()
MBroek 0:9e053ed05c69 194 {
MBroek 0:9e053ed05c69 195 pc.baud(SERIALBAUD);
MBroek 0:9e053ed05c69 196 pc.printf("Starting");
MBroek 0:9e053ed05c69 197
MBroek 4:6524b198721f 198 //test_button.fall(&); // Activeert test button
MBroek 0:9e053ed05c69 199 kill_switch.fall(&emergency_stop); // Activeert kill switch
MBroek 9:f735baee0c2b 200 test_button.fall(&motor_switch); // Switcht motoren
MBroek 0:9e053ed05c69 201
MBroek 4:6524b198721f 202 test_ticker.attach(&flag1_activate,0.1); // Activeert de go-flag van motor positie
MBroek 4:6524b198721f 203 hidscope_ticker.attach(&flag2_activate,0.01);
MBroek 4:6524b198721f 204
MBroek 0:9e053ed05c69 205 while(safe){ // Draait loop zolang alles veilig is.
MBroek 4:6524b198721f 206 if (flag1){
MBroek 4:6524b198721f 207 flag1 = false;
MBroek 9:f735baee0c2b 208 if (!which_motor){ // als which_motor=0 gaat motor 1 lopen
MBroek 9:f735baee0c2b 209 double PID_output_1 = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev, error_int, Kp_1, Kd_1, Ki_1);
MBroek 9:f735baee0c2b 210 set_motor1(PID_output_1);
MBroek 9:f735baee0c2b 211 }
MBroek 9:f735baee0c2b 212 else{
MBroek 9:f735baee0c2b 213 double PID_output_2 = PID_controller(get_reference_position_m2(vrijheid_m2_meter), get_position_m1(meter_per_count), error_prev, error_int, Kp_2, Kd_2, Ki_2);
MBroek 9:f735baee0c2b 214 set_motor2(PID_output_2);
MBroek 9:f735baee0c2b 215 }
MBroek 4:6524b198721f 216 }
MBroek 4:6524b198721f 217 if (flag2){
MBroek 4:6524b198721f 218 flag2 = false;
MBroek 9:f735baee0c2b 219 set_scope(PID_output_2, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count));
MBroek 4:6524b198721f 220 }
MBroek 0:9e053ed05c69 221 }
MBroek 0:9e053ed05c69 222
MBroek 0:9e053ed05c69 223 motor1_speed_pin = 0; //Dit zet de motoren uit nadat de kill switch is gebruikt
MBroek 0:9e053ed05c69 224 motor2_speed_pin = 0;
MBroek 0:9e053ed05c69 225 }
MBroek 0:9e053ed05c69 226
MBroek 0:9e053ed05c69 227
MBroek 0:9e053ed05c69 228
MBroek 0:9e053ed05c69 229
MBroek 0:9e053ed05c69 230
MBroek 0:9e053ed05c69 231
MBroek 0:9e053ed05c69 232
MBroek 0:9e053ed05c69 233
MBroek 0:9e053ed05c69 234
MBroek 0:9e053ed05c69 235
MBroek 0:9e053ed05c69 236
MBroek 0:9e053ed05c69 237
MBroek 0:9e053ed05c69 238
MBroek 0:9e053ed05c69 239
MBroek 0:9e053ed05c69 240
MBroek 0:9e053ed05c69 241
MBroek 0:9e053ed05c69 242
MBroek 0:9e053ed05c69 243
MBroek 0:9e053ed05c69 244
MBroek 0:9e053ed05c69 245
MBroek 0:9e053ed05c69 246
MBroek 0:9e053ed05c69 247
MBroek 0:9e053ed05c69 248
MBroek 0:9e053ed05c69 249
MBroek 0:9e053ed05c69 250
MBroek 0:9e053ed05c69 251
MBroek 0:9e053ed05c69 252
MBroek 0:9e053ed05c69 253
MBroek 0:9e053ed05c69 254
MBroek 0:9e053ed05c69 255
MBroek 0:9e053ed05c69 256
MBroek 0:9e053ed05c69 257
MBroek 0:9e053ed05c69 258
MBroek 0:9e053ed05c69 259
MBroek 0:9e053ed05c69 260
MBroek 0:9e053ed05c69 261
MBroek 0:9e053ed05c69 262
MBroek 0:9e053ed05c69 263
MBroek 0:9e053ed05c69 264
MBroek 0:9e053ed05c69 265
MBroek 0:9e053ed05c69 266
MBroek 0:9e053ed05c69 267
MBroek 0:9e053ed05c69 268
MBroek 0:9e053ed05c69 269
MBroek 0:9e053ed05c69 270
MBroek 0:9e053ed05c69 271
MBroek 0:9e053ed05c69 272
MBroek 0:9e053ed05c69 273
MBroek 0:9e053ed05c69 274
MBroek 0:9e053ed05c69 275
MBroek 0:9e053ed05c69 276
MBroek 0:9e053ed05c69 277
MBroek 0:9e053ed05c69 278
MBroek 0:9e053ed05c69 279