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@1:5b3fa8e47e8b, 2016-10-13 (annotated)

Committer:
MBroek
Date:
Thu Oct 13 09:23:04 2016 +0000
Revision:
1:5b3fa8e47e8b
Parent:
0:9e053ed05c69
Child:
2:6bef5397e8a9
Dit dit nog steeds niks. Ingevoerd zijn (werkende) kill-switch, positiefinder motor 1 en 2, reference position finder en de PID-controller.

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 0:9e053ed05c69 10
MBroek 0:9e053ed05c69 11
MBroek 0:9e053ed05c69 12 // Definieren van de Serial en Encoder -------------------------------
MBroek 0:9e053ed05c69 13 MODSERIAL pc(USBTX, USBRX);
MBroek 0:9e053ed05c69 14 #define SERIALBAUD 115200
MBroek 0:9e053ed05c69 15
MBroek 0:9e053ed05c69 16 QEI encoder_motor1(D10,D11,NC,64);
MBroek 0:9e053ed05c69 17 QEI encoder_motor2(D12,D13,NC,64);
MBroek 0:9e053ed05c69 18
MBroek 0:9e053ed05c69 19
MBroek 0:9e053ed05c69 20 // Definieren van de Motorpins ---------------------------------------
MBroek 0:9e053ed05c69 21 DigitalOut motor1_direction_pin(D7);
MBroek 0:9e053ed05c69 22 PwmOut motor1_speed_pin(D6);
MBroek 0:9e053ed05c69 23
MBroek 0:9e053ed05c69 24 DigitalOut motor2_direction_pin(D4);
MBroek 0:9e053ed05c69 25 PwmOut motor2_speed_pin(D5);
MBroek 0:9e053ed05c69 26
MBroek 0:9e053ed05c69 27
MBroek 0:9e053ed05c69 28 //Definieren van bord-elementen --------------------------------------
MBroek 0:9e053ed05c69 29 InterruptIn kill_switch(D7);
MBroek 0:9e053ed05c69 30 InterruptIn test_button(D6);
MBroek 1:5b3fa8e47e8b 31 AnalogIn pot1(A1); // Dit is de gesimuleerde emg1
MBroek 1:5b3fa8e47e8b 32 AnalogIn pot2(A2); // Dit is de gesimuleerde emg2
MBroek 0:9e053ed05c69 33
MBroek 0:9e053ed05c69 34
MBroek 0:9e053ed05c69 35
MBroek 0:9e053ed05c69 36 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
MBroek 0:9e053ed05c69 37 volatile bool safe = true; // Conditie voor de while-loop in main
MBroek 0:9e053ed05c69 38
MBroek 0:9e053ed05c69 39 int position_motor1;
MBroek 0:9e053ed05c69 40 int counts_motor1;
MBroek 0:9e053ed05c69 41
MBroek 0:9e053ed05c69 42 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 0:9e053ed05c69 43 const double rad_per_count = pi/64.0; // Aantal rad per puls uit encoder
MBroek 0:9e053ed05c69 44
MBroek 1:5b3fa8e47e8b 45 const double meter_per_count = 1; // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
MBroek 1:5b3fa8e47e8b 46
MBroek 0:9e053ed05c69 47 double error_prev = 0.00000; // Initiele error derivative waardes
MBroek 0:9e053ed05c69 48 double error_prev = 0.00000;
MBroek 0:9e053ed05c69 49
MBroek 0:9e053ed05c69 50 double error1_int = 0.d; // Initiele error integral waardes
MBroek 0:9e053ed05c69 51 double error2_int = 0.d;
MBroek 0:9e053ed05c69 52
MBroek 0:9e053ed05c69 53 const double T_getpos = 0.01; // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
MBroek 0:9e053ed05c69 54
MBroek 0:9e053ed05c69 55
MBroek 0:9e053ed05c69 56
MBroek 0:9e053ed05c69 57
MBroek 0:9e053ed05c69 58
MBroek 0:9e053ed05c69 59 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 0:9e053ed05c69 60
MBroek 0:9e053ed05c69 61 // De emergency break ------------------------------------------------
MBroek 0:9e053ed05c69 62 void emergency_stop(){
MBroek 0:9e053ed05c69 63 safe = false;
MBroek 0:9e053ed05c69 64 pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
MBroek 0:9e053ed05c69 65 }
MBroek 0:9e053ed05c69 66
MBroek 1:5b3fa8e47e8b 67
MBroek 1:5b3fa8e47e8b 68 // Functie voor het vinden van de positie van motor 1 ---------------------
MBroek 1:5b3fa8e47e8b 69 double get_position_m1(const double radpercount){ //returned de positie van de motor in radialen (positie vd arm in radialen)
MBroek 0:9e053ed05c69 70 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 1:5b3fa8e47e8b 71 return radpercount * counts1; // rekent positie motor1 uit en geeft deze als output vd functie
MBroek 1:5b3fa8e47e8b 72 }
MBroek 1:5b3fa8e47e8b 73
MBroek 1:5b3fa8e47e8b 74
MBroek 1:5b3fa8e47e8b 75 // Functie voor vinden van de positie van motor 2 -----------------------------
MBroek 1:5b3fa8e47e8b 76 double get_position_m2(const double meterpercount){ // returned de positie van het karretje in meter
MBroek 1:5b3fa8e47e8b 77 counts2 = encoder_motor2.getPulses(); // leest aantal pulses vd encoder af
MBroek 1:5b3fa8e47e8b 78 return meterpercount * counts2; // rekent de positie van het karretje uit en geeft dit als output vd functie
MBroek 1:5b3fa8e47e8b 79 }
MBroek 1:5b3fa8e47e8b 80
MBroek 1:5b3fa8e47e8b 81
MBroek 1:5b3fa8e47e8b 82 // Functie voor het vinden van de reference position van motor 1 en 2 --------------------
MBroek 1:5b3fa8e47e8b 83 double get_reference_position_m1(double aantal_rad, double aantal_meter){
MBroek 1:5b3fa8e47e8b 84 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 85 return ref_pos * aantal_rad; // Aantal rad is hoeveelheid radialen van middelpunt tot minima
MBroek 1:5b3fa8e47e8b 86 return ref_pos * aantal_meter // Aantal meter is de helft van de lengte die het karretje kan bewegen
MBroek 1:5b3fa8e47e8b 87 }
MBroek 0:9e053ed05c69 88
MBroek 0:9e053ed05c69 89
MBroek 0:9e053ed05c69 90
MBroek 1:5b3fa8e47e8b 91 // De PID-controller voor de motors -------------------------------------------------
MBroek 1:5b3fa8e47e8b 92 double PID_controller(double ref_pos, double mot_pos, double &error_prev, double &error_int){
MBroek 0:9e053ed05c69 93 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 0:9e053ed05c69 94 double error_dif = (error-error_prev)/T_getpos // De error differentieren
MBroek 1:5b3fa8e47e8b 95 //error_dif = ..... // Filter biquad poep
MBroek 0:9e053ed05c69 96 error_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 0:9e053ed05c69 97 error_int = error_int + T_getpos*error; // De error integreren
MBroek 0:9e053ed05c69 98 return Kp*error + Ki*error_int + Kd*error_dif // De uiteindelijke PID output
MBroek 0:9e053ed05c69 99 }
MBroek 0:9e053ed05c69 100
MBroek 0:9e053ed05c69 101
MBroek 0:9e053ed05c69 102
MBroek 0:9e053ed05c69 103
MBroek 0:9e053ed05c69 104 // DE MAIN =================================================================================================================
MBroek 0:9e053ed05c69 105 int main()
MBroek 0:9e053ed05c69 106 {
MBroek 0:9e053ed05c69 107 pc.baud(SERIALBAUD);
MBroek 0:9e053ed05c69 108 pc.printf("Starting");
MBroek 0:9e053ed05c69 109
MBroek 0:9e053ed05c69 110 test_button.fall(&PID_controller); // Activeert test button
MBroek 0:9e053ed05c69 111 kill_switch.fall(&emergency_stop); // Activeert kill switch
MBroek 0:9e053ed05c69 112
MBroek 0:9e053ed05c69 113 while(safe){ // Draait loop zolang alles veilig is.
MBroek 0:9e053ed05c69 114 }
MBroek 0:9e053ed05c69 115
MBroek 0:9e053ed05c69 116 motor1_speed_pin = 0; //Dit zet de motoren uit nadat de kill switch is gebruikt
MBroek 0:9e053ed05c69 117 motor2_speed_pin = 0;
MBroek 0:9e053ed05c69 118 }
MBroek 0:9e053ed05c69 119
MBroek 0:9e053ed05c69 120
MBroek 0:9e053ed05c69 121
MBroek 0:9e053ed05c69 122
MBroek 0:9e053ed05c69 123
MBroek 0:9e053ed05c69 124
MBroek 0:9e053ed05c69 125
MBroek 0:9e053ed05c69 126
MBroek 0:9e053ed05c69 127
MBroek 0:9e053ed05c69 128
MBroek 0:9e053ed05c69 129
MBroek 0:9e053ed05c69 130
MBroek 0:9e053ed05c69 131
MBroek 0:9e053ed05c69 132
MBroek 0:9e053ed05c69 133
MBroek 0:9e053ed05c69 134
MBroek 0:9e053ed05c69 135
MBroek 0:9e053ed05c69 136
MBroek 0:9e053ed05c69 137
MBroek 0:9e053ed05c69 138
MBroek 0:9e053ed05c69 139
MBroek 0:9e053ed05c69 140
MBroek 0:9e053ed05c69 141
MBroek 0:9e053ed05c69 142
MBroek 0:9e053ed05c69 143
MBroek 0:9e053ed05c69 144
MBroek 0:9e053ed05c69 145
MBroek 0:9e053ed05c69 146
MBroek 0:9e053ed05c69 147
MBroek 0:9e053ed05c69 148
MBroek 0:9e053ed05c69 149
MBroek 0:9e053ed05c69 150
MBroek 0:9e053ed05c69 151
MBroek 0:9e053ed05c69 152
MBroek 0:9e053ed05c69 153
MBroek 0:9e053ed05c69 154
MBroek 0:9e053ed05c69 155
MBroek 0:9e053ed05c69 156
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