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@15:3f3d87513a6b, 2016-10-23 (annotated)

Committer:
MBroek
Date:
Sun Oct 23 09:42:26 2016 +0000
Revision:
15:3f3d87513a6b
Parent:
14:f51d51395803
Child:
16:5a749b319276
opgeruimde main-loop, werkende initialisatie voor motor1. Is weer stabiel. Uiteindelijk PID coefficienten checken met robot.

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 13:b0d3c547bf2f 38 InterruptIn start_button(SW2);
MBroek 13:b0d3c547bf2f 39 InterruptIn initializing_stopper(SW3);
MBroek 13:b0d3c547bf2f 40
MBroek 13:b0d3c547bf2f 41 DigitalOut ledred(LED_RED, 1);
MBroek 13:b0d3c547bf2f 42 DigitalOut ledgreen(LED_GREEN, 1);
MBroek 13:b0d3c547bf2f 43
MBroek 0:9e053ed05c69 44
MBroek 4:6524b198721f 45 //Definieren van de tickers ------------------------------------------
MBroek 4:6524b198721f 46 Ticker test_ticker;
MBroek 4:6524b198721f 47 Ticker hidscope_ticker;
MBroek 4:6524b198721f 48
MBroek 4:6524b198721f 49
MBroek 0:9e053ed05c69 50
MBroek 0:9e053ed05c69 51 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
MBroek 4:6524b198721f 52
MBroek 15:3f3d87513a6b 53 enum states_enum{off, init_m1, init_m2, finish_init, run}; // De states waar de main loop doorheen loopt, off is uit, init is intialiseren en run is het aanturen vd motoren met emg.
MBroek 14:f51d51395803 54 states_enum states = off;
MBroek 14:f51d51395803 55
MBroek 13:b0d3c547bf2f 56 bool starting = false; // conditie om programma te starten
MBroek 13:b0d3c547bf2f 57
MBroek 13:b0d3c547bf2f 58
MBroek 13:b0d3c547bf2f 59 double error_prev_1_in = 0.0, error_int_1_in = 0.0; // De error waardes voor het initialiseren
MBroek 13:b0d3c547bf2f 60 double PID_output_1_in = 0.0; // De PID output voor het initialiseren
MBroek 13:b0d3c547bf2f 61 bool initializing = true; // conditie voor het initialiseren
MBroek 13:b0d3c547bf2f 62
MBroek 13:b0d3c547bf2f 63
MBroek 0:9e053ed05c69 64 volatile bool safe = true; // Conditie voor de while-loop in main
MBroek 0:9e053ed05c69 65
MBroek 4:6524b198721f 66 double position_motor1;
MBroek 4:6524b198721f 67
MBroek 4:6524b198721f 68 int counts1; // Pulses van motoren
MBroek 4:6524b198721f 69 int counts2;
MBroek 0:9e053ed05c69 70
MBroek 0:9e053ed05c69 71 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 15:3f3d87513a6b 72 const double rad_per_count = 2.0*pi/8400.0; // Aantal rad per puls uit encoder
MBroek 0:9e053ed05c69 73
MBroek 11:91613b22bc00 74 const double radius_tandwiel = 1.0;
MBroek 9:f735baee0c2b 75 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 76
MBroek 4:6524b198721f 77 double error1_int = 0.00000; // Initiele error integral waardes
MBroek 4:6524b198721f 78 double error2_int = 0.00000;
MBroek 0:9e053ed05c69 79
MBroek 0:9e053ed05c69 80 const double T_getpos = 0.01; // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
MBroek 0:9e053ed05c69 81
MBroek 4:6524b198721f 82 volatile bool flag1 = false, flag2 = false; // De flags voor de functies aangeroepen door de tickers
MBroek 4:6524b198721f 83
MBroek 15:3f3d87513a6b 84 const double Kp_1 = 0.1; // De PID variablen voor motor 1
MBroek 15:3f3d87513a6b 85 const double Kd_1 = 0.01;
MBroek 15:3f3d87513a6b 86 const double Ki_1 = 0.01;
MBroek 4:6524b198721f 87
MBroek 9:f735baee0c2b 88 const double Kp_2 = 1.0000000; // De PID variablen voor motor 2
MBroek 9:f735baee0c2b 89 const double Kd_2 = 0.1;
MBroek 9:f735baee0c2b 90 const double Ki_2 = 0.3;
MBroek 5:c510ab61af28 91
MBroek 9:f735baee0c2b 92 const double vrijheid_m1_rad = 0.5*pi; // Dit is de uiterste postitie vd arm in radialen
MBroek 9:f735baee0c2b 93 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 94
MBroek 12:8aba69d8d0d0 95 const double starting_pos_m1 = 0.5*pi; // Startin posities van de motoren
MBroek 13:b0d3c547bf2f 96 const double starting_pos_m2 = 0.25;
MBroek 11:91613b22bc00 97
MBroek 13:b0d3c547bf2f 98 double reference_position_motor1 = 0; // Dit zijn de eerste posities waar de motoren heen willen bij de initialisatie
MBroek 10:755b9228cc42 99 double reference_position_motor2 = 0;
MBroek 9:f735baee0c2b 100
MBroek 13:b0d3c547bf2f 101 double PID_output_1 = 0.0; // De eerste PID_output, deze is nul anders gaan de motoren draaien
MBroek 13:b0d3c547bf2f 102 double PID_output_2 = 0.0;
MBroek 5:c510ab61af28 103
MBroek 13:b0d3c547bf2f 104 double error_prev_1 = 0.0; // De initiele previous error
MBroek 13:b0d3c547bf2f 105 double error_int_1 = 0.0; // De initiele intergral error
MBroek 10:755b9228cc42 106
MBroek 13:b0d3c547bf2f 107 double error_prev_2 = 0.0;
MBroek 13:b0d3c547bf2f 108 double error_int_2 = 0.0;
MBroek 5:c510ab61af28 109
MBroek 15:3f3d87513a6b 110 //bool which_motor = false;
MBroek 15:3f3d87513a6b 111 enum which_motor{motor1, motor2};
MBroek 15:3f3d87513a6b 112 which_motor motor_that_runs = motor1;
MBroek 15:3f3d87513a6b 113
MBroek 9:f735baee0c2b 114
MBroek 0:9e053ed05c69 115
MBroek 0:9e053ed05c69 116
MBroek 0:9e053ed05c69 117
MBroek 0:9e053ed05c69 118
MBroek 0:9e053ed05c69 119 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 0:9e053ed05c69 120
MBroek 13:b0d3c547bf2f 121 // De program starter ------------------------------------------------
MBroek 13:b0d3c547bf2f 122 void start_program(){
MBroek 15:3f3d87513a6b 123 states = init_m1;
MBroek 13:b0d3c547bf2f 124 pc.printf("Starting\n\r");
MBroek 13:b0d3c547bf2f 125 }
MBroek 13:b0d3c547bf2f 126
MBroek 13:b0d3c547bf2f 127
MBroek 0:9e053ed05c69 128 // De emergency break ------------------------------------------------
MBroek 0:9e053ed05c69 129 void emergency_stop(){
MBroek 0:9e053ed05c69 130 safe = false;
MBroek 0:9e053ed05c69 131 pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
MBroek 0:9e053ed05c69 132 }
MBroek 0:9e053ed05c69 133
MBroek 1:5b3fa8e47e8b 134
MBroek 13:b0d3c547bf2f 135 // De initialiseer beeindiger knop ------------------------------------------------
MBroek 13:b0d3c547bf2f 136 void stop_initializing(){
MBroek 15:3f3d87513a6b 137 states = run;
MBroek 15:3f3d87513a6b 138 ledred.write(1);
MBroek 15:3f3d87513a6b 139 encoder_motor1.reset();
MBroek 13:b0d3c547bf2f 140 pc.printf("Initializing ended\r\n");
MBroek 13:b0d3c547bf2f 141 }
MBroek 13:b0d3c547bf2f 142
MBroek 15:3f3d87513a6b 143 /*
MBroek 15:3f3d87513a6b 144 // De statechanger -----------------------------------------------------------
MBroek 15:3f3d87513a6b 145 void state_changer(){
MBroek 15:3f3d87513a6b 146 if(states == off){
MBroek 15:3f3d87513a6b 147 states = init_m1;
MBroek 15:3f3d87513a6b 148 }
MBroek 15:3f3d87513a6b 149 ()
MBroek 15:3f3d87513a6b 150 */
MBroek 13:b0d3c547bf2f 151
MBroek 9:f735baee0c2b 152 // Functie voor het switchen tussen de motors ------------------------------
MBroek 9:f735baee0c2b 153 void motor_switch(){
MBroek 15:3f3d87513a6b 154 if(motor_that_runs == motor1){
MBroek 15:3f3d87513a6b 155 motor_that_runs = motor2;
MBroek 15:3f3d87513a6b 156 }
MBroek 15:3f3d87513a6b 157 else{
MBroek 15:3f3d87513a6b 158 motor_that_runs = motor1;
MBroek 15:3f3d87513a6b 159 }
MBroek 15:3f3d87513a6b 160
MBroek 9:f735baee0c2b 161 }
MBroek 9:f735baee0c2b 162
MBroek 9:f735baee0c2b 163
MBroek 1:5b3fa8e47e8b 164 // Functie voor het vinden van de positie van motor 1 ---------------------
MBroek 13:b0d3c547bf2f 165 double get_position_m1(const double radpercount){ //returned de positie van de motor in radialen (positie vd arm in radialen)
MBroek 0:9e053ed05c69 166 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 13:b0d3c547bf2f 167 return radpercount * counts1; // rekent positie motor1 uit en geeft deze als output vd functie
MBroek 1:5b3fa8e47e8b 168 }
MBroek 1:5b3fa8e47e8b 169
MBroek 1:5b3fa8e47e8b 170
MBroek 1:5b3fa8e47e8b 171 // Functie voor vinden van de positie van motor 2 -----------------------------
MBroek 13:b0d3c547bf2f 172 double get_position_m2(const double meterpercount){ // returned de positie van het karretje in meter
MBroek 1:5b3fa8e47e8b 173 counts2 = encoder_motor2.getPulses(); // leest aantal pulses vd encoder af
MBroek 13:b0d3c547bf2f 174 return meterpercount * counts2; // rekent de positie van het karretje uit en geeft dit als output vd functie
MBroek 1:5b3fa8e47e8b 175 }
MBroek 1:5b3fa8e47e8b 176
MBroek 1:5b3fa8e47e8b 177
MBroek 5:c510ab61af28 178 // Functie voor het vinden van de reference position van motor 1 --------------------
MBroek 13:b0d3c547bf2f 179 double get_reference_position_m1(const double aantal_rad){
MBroek 4:6524b198721f 180 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 13:b0d3c547bf2f 181 return ref_pos * aantal_rad; // Aantal rad is de uiterste positie vd arm in radialen
MBroek 5:c510ab61af28 182 }
MBroek 5:c510ab61af28 183
MBroek 5:c510ab61af28 184
MBroek 5:c510ab61af28 185 // Functie voor het vinden van de reference position van motor 2 --------------------
MBroek 13:b0d3c547bf2f 186 double get_reference_position_m2(const double aantal_meter){
MBroek 5:c510ab61af28 187 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 13:b0d3c547bf2f 188 return ref_pos * aantal_meter; // Aantal meter is hoeveelheid radialen van middelpunt tot minima.
MBroek 1:5b3fa8e47e8b 189 }
MBroek 0:9e053ed05c69 190
MBroek 3:581c5918b590 191
MBroek 3:581c5918b590 192 // HIDScope functie ----------------------------------------------------------------------
MBroek 7:435f984781ab 193 void set_scope(double input1, double input2, double input3){
MBroek 7:435f984781ab 194 scope.set(0, input1);
MBroek 7:435f984781ab 195 scope.set(1, input2);
MBroek 7:435f984781ab 196 scope.set(2, input3);
MBroek 3:581c5918b590 197 scope.send();
MBroek 3:581c5918b590 198 }
MBroek 3:581c5918b590 199
MBroek 0:9e053ed05c69 200
MBroek 1:5b3fa8e47e8b 201 // De PID-controller voor de motors -------------------------------------------------
MBroek 6:e0857842e8cd 202 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 203 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 6:e0857842e8cd 204 double error_dif = (error-e_prev)/T_getpos; // De error differentieren
MBroek 1:5b3fa8e47e8b 205 //error_dif = ..... // Filter biquad poep
MBroek 6:e0857842e8cd 206 e_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 6:e0857842e8cd 207 e_int = e_int + T_getpos*error; // De error integreren
MBroek 6:e0857842e8cd 208 return Kp*error + Ki*e_int + Kd*error_dif; // De uiteindelijke PID output
MBroek 0:9e053ed05c69 209 }
MBroek 0:9e053ed05c69 210
MBroek 0:9e053ed05c69 211
MBroek 2:6bef5397e8a9 212 // Motor 1 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 213 void set_motor1(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 214 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 215 motor1_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 216 }
MBroek 2:6bef5397e8a9 217 else {
MBroek 2:6bef5397e8a9 218 motor1_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 219 }
MBroek 2:6bef5397e8a9 220 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 221 motor_input = 1;
MBroek 2:6bef5397e8a9 222 }
MBroek 2:6bef5397e8a9 223 motor1_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 224 }
MBroek 2:6bef5397e8a9 225
MBroek 2:6bef5397e8a9 226
MBroek 2:6bef5397e8a9 227 // Motor 2 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 228 void set_motor2(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 229 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 230 motor2_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 231 }
MBroek 2:6bef5397e8a9 232 else {
MBroek 2:6bef5397e8a9 233 motor2_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 234 }
MBroek 2:6bef5397e8a9 235 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 236 motor_input = 1;
MBroek 2:6bef5397e8a9 237 }
MBroek 2:6bef5397e8a9 238 motor2_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 239 }
MBroek 2:6bef5397e8a9 240
MBroek 0:9e053ed05c69 241
MBroek 4:6524b198721f 242 // De go-flag functies -----------------------------------------------------------------
MBroek 4:6524b198721f 243 void flag1_activate(){flag1=true;} // Activeert de flag
MBroek 14:f51d51395803 244 void flag2_activate(){flag2=true;}
MBroek 14:f51d51395803 245
MBroek 14:f51d51395803 246
MBroek 14:f51d51395803 247 // De initialiseren functie ------------------------------------------------------------
MBroek 14:f51d51395803 248 void initialise_m1(){
MBroek 14:f51d51395803 249 if (flag1){
MBroek 14:f51d51395803 250 flag1 = false;
MBroek 14:f51d51395803 251 ledred = !ledred;
MBroek 14:f51d51395803 252 PID_output_1_in = PID_controller(reference_position_motor1, get_position_m1(rad_per_count)+starting_pos_m1, error_prev_1_in, error_int_1_in, Kp_1, Kd_1, Ki_1);
MBroek 14:f51d51395803 253 set_motor1(PID_output_1_in);
MBroek 14:f51d51395803 254 }
MBroek 14:f51d51395803 255 }
MBroek 4:6524b198721f 256
MBroek 4:6524b198721f 257
MBroek 15:3f3d87513a6b 258 // De functie die de motoren aanstuurt -------------------------------------------------
MBroek 15:3f3d87513a6b 259 void running_motors(){
MBroek 15:3f3d87513a6b 260 if (flag1){
MBroek 15:3f3d87513a6b 261 flag1 = false;
MBroek 15:3f3d87513a6b 262 ledgreen = !ledgreen;
MBroek 15:3f3d87513a6b 263 switch (motor_that_runs){
MBroek 15:3f3d87513a6b 264 case motor1 : // In deze case draait alleen motor 1
MBroek 15:3f3d87513a6b 265 PID_output_1 = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
MBroek 15:3f3d87513a6b 266 set_motor1(PID_output_1);
MBroek 15:3f3d87513a6b 267 break;
MBroek 15:3f3d87513a6b 268 case motor2 :
MBroek 15:3f3d87513a6b 269 PID_output_2 = PID_controller(get_reference_position_m2(vrijheid_m2_meter), get_position_m2(meter_per_count), error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
MBroek 15:3f3d87513a6b 270 set_motor2(PID_output_2);
MBroek 15:3f3d87513a6b 271 break;
MBroek 15:3f3d87513a6b 272 }
MBroek 15:3f3d87513a6b 273 }
MBroek 15:3f3d87513a6b 274 }
MBroek 15:3f3d87513a6b 275
MBroek 15:3f3d87513a6b 276
MBroek 15:3f3d87513a6b 277 // De HIDscope debug functie ----------------------------------------------------------------
MBroek 15:3f3d87513a6b 278 void call_HIDscope(double input_1, double input_2, double input_3){
MBroek 15:3f3d87513a6b 279 if (flag2){
MBroek 15:3f3d87513a6b 280 flag2 = false;
MBroek 15:3f3d87513a6b 281 set_scope(input_1, input_2, input_3);
MBroek 15:3f3d87513a6b 282 }
MBroek 15:3f3d87513a6b 283 }
MBroek 15:3f3d87513a6b 284
MBroek 15:3f3d87513a6b 285
MBroek 15:3f3d87513a6b 286
MBroek 0:9e053ed05c69 287
MBroek 0:9e053ed05c69 288 // DE MAIN =================================================================================================================
MBroek 0:9e053ed05c69 289 int main()
MBroek 0:9e053ed05c69 290 {
MBroek 0:9e053ed05c69 291 pc.baud(SERIALBAUD);
MBroek 0:9e053ed05c69 292
MBroek 0:9e053ed05c69 293 kill_switch.fall(&emergency_stop); // Activeert kill switch
MBroek 9:f735baee0c2b 294 test_button.fall(&motor_switch); // Switcht motoren
MBroek 13:b0d3c547bf2f 295 start_button.fall(&start_program);
MBroek 13:b0d3c547bf2f 296 initializing_stopper.fall(&stop_initializing);
MBroek 13:b0d3c547bf2f 297
MBroek 0:9e053ed05c69 298
MBroek 4:6524b198721f 299 test_ticker.attach(&flag1_activate,0.1); // Activeert de go-flag van motor positie
MBroek 13:b0d3c547bf2f 300 hidscope_ticker.attach(&flag2_activate,0.01);
MBroek 13:b0d3c547bf2f 301
MBroek 14:f51d51395803 302 while(safe){
MBroek 14:f51d51395803 303 switch(states){
MBroek 14:f51d51395803 304 case off : // Dan staat het programma stil en gebeurt er niks
MBroek 14:f51d51395803 305 break;
MBroek 14:f51d51395803 306 case init_m1 : // Hier voert hij alleen het initialiseren van motor 1 uit
MBroek 15:3f3d87513a6b 307 initialise_m1();
MBroek 14:f51d51395803 308 break;
MBroek 15:3f3d87513a6b 309 case run :
MBroek 15:3f3d87513a6b 310 running_motors();
MBroek 15:3f3d87513a6b 311 break;
MBroek 14:f51d51395803 312
MBroek 15:3f3d87513a6b 313 }
MBroek 15:3f3d87513a6b 314 call_HIDscope(PID_output_1_in, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(meter_per_count));
MBroek 15:3f3d87513a6b 315 }
MBroek 15:3f3d87513a6b 316 motor1_speed_pin = 0; //Dit zet de motoren uit nadat de kill switch is gebruikt
MBroek 15:3f3d87513a6b 317 motor2_speed_pin = 0;
MBroek 15:3f3d87513a6b 318 }
MBroek 14:f51d51395803 319
MBroek 14:f51d51395803 320
MBroek 14:f51d51395803 321
MBroek 14:f51d51395803 322
MBroek 14:f51d51395803 323
MBroek 14:f51d51395803 324
MBroek 14:f51d51395803 325
MBroek 14:f51d51395803 326
MBroek 0:9e053ed05c69 327
MBroek 0:9e053ed05c69 328
MBroek 0:9e053ed05c69 329
MBroek 0:9e053ed05c69 330
MBroek 0:9e053ed05c69 331
MBroek 0:9e053ed05c69 332
MBroek 0:9e053ed05c69 333
MBroek 0:9e053ed05c69 334
MBroek 0:9e053ed05c69 335
MBroek 0:9e053ed05c69 336
MBroek 0:9e053ed05c69 337
MBroek 0:9e053ed05c69 338
MBroek 0:9e053ed05c69 339
MBroek 0:9e053ed05c69 340
MBroek 0:9e053ed05c69 341
MBroek 0:9e053ed05c69 342
MBroek 0:9e053ed05c69 343
MBroek 0:9e053ed05c69 344
MBroek 0:9e053ed05c69 345
MBroek 0:9e053ed05c69 346
MBroek 0:9e053ed05c69 347
MBroek 0:9e053ed05c69 348
MBroek 0:9e053ed05c69 349
MBroek 0:9e053ed05c69 350
MBroek 0:9e053ed05c69 351
MBroek 0:9e053ed05c69 352
MBroek 0:9e053ed05c69 353
MBroek 0:9e053ed05c69 354
MBroek 0:9e053ed05c69 355
MBroek 0:9e053ed05c69 356
MBroek 0:9e053ed05c69 357
MBroek 0:9e053ed05c69 358
MBroek 0:9e053ed05c69 359
MBroek 0:9e053ed05c69 360
MBroek 0:9e053ed05c69 361
MBroek 0:9e053ed05c69 362
MBroek 0:9e053ed05c69 363
MBroek 0:9e053ed05c69 364
MBroek 0:9e053ed05c69 365
MBroek 0:9e053ed05c69 366
MBroek 0:9e053ed05c69 367
MBroek 0:9e053ed05c69 368
MBroek 0:9e053ed05c69 369
MBroek 0:9e053ed05c69 370
MBroek 0:9e053ed05c69 371
MBroek 0:9e053ed05c69 372
MBroek 0:9e053ed05c69 373
MBroek 0:9e053ed05c69 374
MBroek 0:9e053ed05c69 375
MBroek 0:9e053ed05c69 376
MBroek 0:9e053ed05c69 377
MBroek 0:9e053ed05c69 378
MBroek 0:9e053ed05c69 379
MBroek 0:9e053ed05c69 380