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@0:9e053ed05c69, 2016-10-11 (annotated)

Committer:
MBroek
Date:
Tue Oct 11 10:21:37 2016 +0000
Revision:
0:9e053ed05c69
Child:
1:5b3fa8e47e8b
Doet niks, is nog niet af.

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 0:9e053ed05c69 31
MBroek 0:9e053ed05c69 32
MBroek 0:9e053ed05c69 33
MBroek 0:9e053ed05c69 34 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
MBroek 0:9e053ed05c69 35 volatile bool safe = true; // Conditie voor de while-loop in main
MBroek 0:9e053ed05c69 36
MBroek 0:9e053ed05c69 37 int position_motor1;
MBroek 0:9e053ed05c69 38 int counts_motor1;
MBroek 0:9e053ed05c69 39
MBroek 0:9e053ed05c69 40 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 0:9e053ed05c69 41 const double rad_per_count = pi/64.0; // Aantal rad per puls uit encoder
MBroek 0:9e053ed05c69 42
MBroek 0:9e053ed05c69 43 double error_prev = 0.00000; // Initiele error derivative waardes
MBroek 0:9e053ed05c69 44 double error_prev = 0.00000;
MBroek 0:9e053ed05c69 45
MBroek 0:9e053ed05c69 46 double error1_int = 0.d; // Initiele error integral waardes
MBroek 0:9e053ed05c69 47 double error2_int = 0.d;
MBroek 0:9e053ed05c69 48
MBroek 0:9e053ed05c69 49 const double T_getpos = 0.01; // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
MBroek 0:9e053ed05c69 50
MBroek 0:9e053ed05c69 51
MBroek 0:9e053ed05c69 52
MBroek 0:9e053ed05c69 53
MBroek 0:9e053ed05c69 54
MBroek 0:9e053ed05c69 55 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 0:9e053ed05c69 56
MBroek 0:9e053ed05c69 57 // De emergency break ------------------------------------------------
MBroek 0:9e053ed05c69 58 void emergency_stop(){
MBroek 0:9e053ed05c69 59 safe = false;
MBroek 0:9e053ed05c69 60 pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
MBroek 0:9e053ed05c69 61 }
MBroek 0:9e053ed05c69 62
MBroek 0:9e053ed05c69 63
MBroek 0:9e053ed05c69 64 double get_positions(){
MBroek 0:9e053ed05c69 65 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 0:9e053ed05c69 66 motor1_position = rad_per_count * counts1; // rekent positie motor1 uit
MBroek 0:9e053ed05c69 67 reference1_position = .... // = constante * emg signaal 1+2
MBroek 0:9e053ed05c69 68
MBroek 0:9e053ed05c69 69
MBroek 0:9e053ed05c69 70
MBroek 0:9e053ed05c69 71 // De PID-controller voor motor 1 -------------------------------------------------
MBroek 0:9e053ed05c69 72 double PID_controller1(double ref_pos, double mot_pos, double &error_prev, double &error_int){
MBroek 0:9e053ed05c69 73 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 0:9e053ed05c69 74 double error_dif = (error-error_prev)/T_getpos // De error differentieren
MBroek 0:9e053ed05c69 75 error_dif = ..... // Filter biquad poep
MBroek 0:9e053ed05c69 76 error_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 0:9e053ed05c69 77 error_int = error_int + T_getpos*error; // De error integreren
MBroek 0:9e053ed05c69 78 return Kp*error + Ki*error_int + Kd*error_dif // De uiteindelijke PID output
MBroek 0:9e053ed05c69 79 }
MBroek 0:9e053ed05c69 80
MBroek 0:9e053ed05c69 81
MBroek 0:9e053ed05c69 82
MBroek 0:9e053ed05c69 83
MBroek 0:9e053ed05c69 84 // DE MAIN =================================================================================================================
MBroek 0:9e053ed05c69 85 int main()
MBroek 0:9e053ed05c69 86 {
MBroek 0:9e053ed05c69 87 pc.baud(SERIALBAUD);
MBroek 0:9e053ed05c69 88 pc.printf("Starting");
MBroek 0:9e053ed05c69 89
MBroek 0:9e053ed05c69 90 test_button.fall(&PID_controller); // Activeert test button
MBroek 0:9e053ed05c69 91 kill_switch.fall(&emergency_stop); // Activeert kill switch
MBroek 0:9e053ed05c69 92
MBroek 0:9e053ed05c69 93 while(safe){ // Draait loop zolang alles veilig is.
MBroek 0:9e053ed05c69 94 }
MBroek 0:9e053ed05c69 95
MBroek 0:9e053ed05c69 96 motor1_speed_pin = 0; //Dit zet de motoren uit nadat de kill switch is gebruikt
MBroek 0:9e053ed05c69 97 motor2_speed_pin = 0;
MBroek 0:9e053ed05c69 98 }
MBroek 0:9e053ed05c69 99
MBroek 0:9e053ed05c69 100
MBroek 0:9e053ed05c69 101
MBroek 0:9e053ed05c69 102
MBroek 0:9e053ed05c69 103
MBroek 0:9e053ed05c69 104
MBroek 0:9e053ed05c69 105
MBroek 0:9e053ed05c69 106
MBroek 0:9e053ed05c69 107
MBroek 0:9e053ed05c69 108
MBroek 0:9e053ed05c69 109
MBroek 0:9e053ed05c69 110
MBroek 0:9e053ed05c69 111
MBroek 0:9e053ed05c69 112
MBroek 0:9e053ed05c69 113
MBroek 0:9e053ed05c69 114
MBroek 0:9e053ed05c69 115
MBroek 0:9e053ed05c69 116
MBroek 0:9e053ed05c69 117
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