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@3:581c5918b590, 2016-10-13 (annotated)

Committer:
MBroek
Date:
Thu Oct 13 15:05:35 2016 +0000
Revision:
3:581c5918b590
Parent:
2:6bef5397e8a9
Child:
4:6524b198721f
Hidscope toegevoegd om functie mee te gaan testen

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 3:581c5918b590 13 HIDScope scope(1)
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 0:9e053ed05c69 39
MBroek 0:9e053ed05c69 40 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
MBroek 0:9e053ed05c69 41 volatile bool safe = true; // Conditie voor de while-loop in main
MBroek 0:9e053ed05c69 42
MBroek 0:9e053ed05c69 43 int position_motor1;
MBroek 0:9e053ed05c69 44 int counts_motor1;
MBroek 0:9e053ed05c69 45
MBroek 0:9e053ed05c69 46 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 0:9e053ed05c69 47 const double rad_per_count = pi/64.0; // Aantal rad per puls uit encoder
MBroek 0:9e053ed05c69 48
MBroek 1:5b3fa8e47e8b 49 const double meter_per_count = 1; // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
MBroek 1:5b3fa8e47e8b 50
MBroek 0:9e053ed05c69 51 double error_prev = 0.00000; // Initiele error derivative waardes
MBroek 0:9e053ed05c69 52 double error_prev = 0.00000;
MBroek 0:9e053ed05c69 53
MBroek 0:9e053ed05c69 54 double error1_int = 0.d; // Initiele error integral waardes
MBroek 0:9e053ed05c69 55 double error2_int = 0.d;
MBroek 0:9e053ed05c69 56
MBroek 0:9e053ed05c69 57 const double T_getpos = 0.01; // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
MBroek 0:9e053ed05c69 58
MBroek 0:9e053ed05c69 59
MBroek 0:9e053ed05c69 60
MBroek 0:9e053ed05c69 61
MBroek 0:9e053ed05c69 62
MBroek 0:9e053ed05c69 63 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 0:9e053ed05c69 64
MBroek 0:9e053ed05c69 65 // De emergency break ------------------------------------------------
MBroek 0:9e053ed05c69 66 void emergency_stop(){
MBroek 0:9e053ed05c69 67 safe = false;
MBroek 0:9e053ed05c69 68 pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
MBroek 0:9e053ed05c69 69 }
MBroek 0:9e053ed05c69 70
MBroek 1:5b3fa8e47e8b 71
MBroek 1:5b3fa8e47e8b 72 // Functie voor het vinden van de positie van motor 1 ---------------------
MBroek 1:5b3fa8e47e8b 73 double get_position_m1(const double radpercount){ //returned de positie van de motor in radialen (positie vd arm in radialen)
MBroek 0:9e053ed05c69 74 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 1:5b3fa8e47e8b 75 return radpercount * counts1; // rekent positie motor1 uit en geeft deze als output vd functie
MBroek 1:5b3fa8e47e8b 76 }
MBroek 1:5b3fa8e47e8b 77
MBroek 1:5b3fa8e47e8b 78
MBroek 1:5b3fa8e47e8b 79 // Functie voor vinden van de positie van motor 2 -----------------------------
MBroek 1:5b3fa8e47e8b 80 double get_position_m2(const double meterpercount){ // returned de positie van het karretje in meter
MBroek 1:5b3fa8e47e8b 81 counts2 = encoder_motor2.getPulses(); // leest aantal pulses vd encoder af
MBroek 1:5b3fa8e47e8b 82 return meterpercount * counts2; // rekent de positie van het karretje uit en geeft dit als output vd functie
MBroek 1:5b3fa8e47e8b 83 }
MBroek 1:5b3fa8e47e8b 84
MBroek 1:5b3fa8e47e8b 85
MBroek 1:5b3fa8e47e8b 86 // Functie voor het vinden van de reference position van motor 1 en 2 --------------------
MBroek 1:5b3fa8e47e8b 87 double get_reference_position_m1(double aantal_rad, double aantal_meter){
MBroek 1:5b3fa8e47e8b 88 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 1:5b3fa8e47e8b 89 return ref_pos * aantal_rad; // Aantal rad is hoeveelheid radialen van middelpunt tot minima
MBroek 3:581c5918b590 90 return ref_pos * aantal_meter; // Aantal meter is de helft van de lengte die het karretje kan bewegen
MBroek 1:5b3fa8e47e8b 91 }
MBroek 0:9e053ed05c69 92
MBroek 3:581c5918b590 93
MBroek 3:581c5918b590 94 // HIDScope functie ----------------------------------------------------------------------
MBroek 3:581c5918b590 95 void set_scope(double input){
MBroek 3:581c5918b590 96 scope.set(0, input);
MBroek 3:581c5918b590 97 scope.send();
MBroek 3:581c5918b590 98 }
MBroek 3:581c5918b590 99
MBroek 0:9e053ed05c69 100
MBroek 1:5b3fa8e47e8b 101 // De PID-controller voor de motors -------------------------------------------------
MBroek 1:5b3fa8e47e8b 102 double PID_controller(double ref_pos, double mot_pos, double &error_prev, double &error_int){
MBroek 0:9e053ed05c69 103 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 0:9e053ed05c69 104 double error_dif = (error-error_prev)/T_getpos // De error differentieren
MBroek 1:5b3fa8e47e8b 105 //error_dif = ..... // Filter biquad poep
MBroek 0:9e053ed05c69 106 error_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 0:9e053ed05c69 107 error_int = error_int + T_getpos*error; // De error integreren
MBroek 0:9e053ed05c69 108 return Kp*error + Ki*error_int + Kd*error_dif // De uiteindelijke PID output
MBroek 0:9e053ed05c69 109 }
MBroek 0:9e053ed05c69 110
MBroek 0:9e053ed05c69 111
MBroek 2:6bef5397e8a9 112 // Motor 1 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 113 void set_motor1(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 114 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 115 motor1_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 116 }
MBroek 2:6bef5397e8a9 117 else {
MBroek 2:6bef5397e8a9 118 motor1_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 119 }
MBroek 2:6bef5397e8a9 120 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 121 motor_input = 1;
MBroek 2:6bef5397e8a9 122 }
MBroek 2:6bef5397e8a9 123 motor1_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 124 }
MBroek 2:6bef5397e8a9 125
MBroek 2:6bef5397e8a9 126
MBroek 2:6bef5397e8a9 127 // Motor 2 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 128 void set_motor2(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 129 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 130 motor2_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 131 }
MBroek 2:6bef5397e8a9 132 else {
MBroek 2:6bef5397e8a9 133 motor2_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 134 }
MBroek 2:6bef5397e8a9 135 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 136 motor_input = 1;
MBroek 2:6bef5397e8a9 137 }
MBroek 2:6bef5397e8a9 138 motor2_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 139 }
MBroek 2:6bef5397e8a9 140
MBroek 0:9e053ed05c69 141
MBroek 0:9e053ed05c69 142
MBroek 0:9e053ed05c69 143 // DE MAIN =================================================================================================================
MBroek 0:9e053ed05c69 144 int main()
MBroek 0:9e053ed05c69 145 {
MBroek 0:9e053ed05c69 146 pc.baud(SERIALBAUD);
MBroek 0:9e053ed05c69 147 pc.printf("Starting");
MBroek 0:9e053ed05c69 148
MBroek 0:9e053ed05c69 149 test_button.fall(&PID_controller); // Activeert test button
MBroek 0:9e053ed05c69 150 kill_switch.fall(&emergency_stop); // Activeert kill switch
MBroek 0:9e053ed05c69 151
MBroek 0:9e053ed05c69 152 while(safe){ // Draait loop zolang alles veilig is.
MBroek 0:9e053ed05c69 153 }
MBroek 0:9e053ed05c69 154
MBroek 0:9e053ed05c69 155 motor1_speed_pin = 0; //Dit zet de motoren uit nadat de kill switch is gebruikt
MBroek 0:9e053ed05c69 156 motor2_speed_pin = 0;
MBroek 0:9e053ed05c69 157 }
MBroek 0:9e053ed05c69 158
MBroek 0:9e053ed05c69 159
MBroek 0:9e053ed05c69 160
MBroek 0:9e053ed05c69 161
MBroek 0:9e053ed05c69 162
MBroek 0:9e053ed05c69 163
MBroek 0:9e053ed05c69 164
MBroek 0:9e053ed05c69 165
MBroek 0:9e053ed05c69 166
MBroek 0:9e053ed05c69 167
MBroek 0:9e053ed05c69 168
MBroek 0:9e053ed05c69 169
MBroek 0:9e053ed05c69 170
MBroek 0:9e053ed05c69 171
MBroek 0:9e053ed05c69 172
MBroek 0:9e053ed05c69 173
MBroek 0:9e053ed05c69 174
MBroek 0:9e053ed05c69 175
MBroek 0:9e053ed05c69 176
MBroek 0:9e053ed05c69 177
MBroek 0:9e053ed05c69 178
MBroek 0:9e053ed05c69 179
MBroek 0:9e053ed05c69 180
MBroek 0:9e053ed05c69 181
MBroek 0:9e053ed05c69 182
MBroek 0:9e053ed05c69 183
MBroek 0:9e053ed05c69 184
MBroek 0:9e053ed05c69 185
MBroek 0:9e053ed05c69 186
MBroek 0:9e053ed05c69 187
MBroek 0:9e053ed05c69 188
MBroek 0:9e053ed05c69 189
MBroek 0:9e053ed05c69 190
MBroek 0:9e053ed05c69 191
MBroek 0:9e053ed05c69 192
MBroek 0:9e053ed05c69 193
MBroek 0:9e053ed05c69 194
MBroek 0:9e053ed05c69 195
MBroek 0:9e053ed05c69 196
MBroek 0:9e053ed05c69 197
MBroek 0:9e053ed05c69 198
MBroek 0:9e053ed05c69 199
MBroek 0:9e053ed05c69 200
MBroek 0:9e053ed05c69 201
MBroek 0:9e053ed05c69 202
MBroek 0:9e053ed05c69 203
MBroek 0:9e053ed05c69 204
MBroek 0:9e053ed05c69 205
MBroek 0:9e053ed05c69 206
MBroek 0:9e053ed05c69 207
MBroek 0:9e053ed05c69 208
MBroek 0:9e053ed05c69 209
MBroek 0:9e053ed05c69 210
MBroek 0:9e053ed05c69 211