Jochem Brouwer
MODSERIAL PID controller edit
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
PID_controller_Motor
by 
BioBitches TN
Revision 0:9e053ed05c69, committed 2016-10-11
- Comitter:
- MBroek
- Date:
- Tue Oct 11 10:21:37 2016 +0000
- Child:
- 1:5b3fa8e47e8b
- Commit message:
- Doet niks, is nog niet af.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Tue Oct 11 10:21:37 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#4737f8a5b018
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Tue Oct 11 10:21:37 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Oct 11 10:21:37 2016 +0000
@@ -0,0 +1,152 @@
+
+
+// HET DEFINIEREN VAN ALLES ==========================================================================================
+
+// Includen van alle libraries ---------------------------------------
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "math.h"
+
+
+// Definieren van de Serial en Encoder -------------------------------
+MODSERIAL pc(USBTX, USBRX);
+#define SERIALBAUD 115200
+
+QEI encoder_motor1(D10,D11,NC,64);
+QEI encoder_motor2(D12,D13,NC,64);
+
+
+// Definieren van de Motorpins ---------------------------------------
+DigitalOut motor1_direction_pin(D7);
+PwmOut motor1_speed_pin(D6);
+
+DigitalOut motor2_direction_pin(D4);
+PwmOut motor2_speed_pin(D5);
+
+
+//Definieren van bord-elementen --------------------------------------
+InterruptIn kill_switch(D7);
+InterruptIn test_button(D6);
+
+
+
+// HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
+volatile bool safe = true; // Conditie voor de while-loop in main
+
+int position_motor1;
+int counts_motor1;
+
+const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
+const double rad_per_count = pi/64.0; // Aantal rad per puls uit encoder
+
+double error_prev = 0.00000; // Initiele error derivative waardes
+double error_prev = 0.00000;
+
+double error1_int = 0.d; // Initiele error integral waardes
+double error2_int = 0.d;
+
+const double T_getpos = 0.01; // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
+
+
+
+
+
+// ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
+
+// De emergency break ------------------------------------------------
+void emergency_stop(){
+ safe = false;
+ pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
+}
+
+
+double get_positions(){
+ counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
+ motor1_position = rad_per_count * counts1; // rekent positie motor1 uit
+ reference1_position = .... // = constante * emg signaal 1+2
+
+
+
+// De PID-controller voor motor 1 -------------------------------------------------
+double PID_controller1(double ref_pos, double mot_pos, double &error_prev, double &error_int){
+ double error = ref_pos - mot_pos; // error uitrekenen
+ double error_dif = (error-error_prev)/T_getpos // De error differentieren
+ error_dif = ..... // Filter biquad poep
+ error_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
+ error_int = error_int + T_getpos*error; // De error integreren
+ return Kp*error + Ki*error_int + Kd*error_dif // De uiteindelijke PID output
+}
+
+
+
+
+// DE MAIN =================================================================================================================
+int main()
+{
+ pc.baud(SERIALBAUD);
+ pc.printf("Starting");
+
+ test_button.fall(&PID_controller); // Activeert test button
+ kill_switch.fall(&emergency_stop); // Activeert kill switch
+
+ while(safe){ // Draait loop zolang alles veilig is.
+ }
+
+ motor1_speed_pin = 0; //Dit zet de motoren uit nadat de kill switch is gebruikt
+ motor2_speed_pin = 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 11 10:21:37 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3 \ No newline at end of file