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@18:6cfe248b2290, 2016-10-24 (annotated)

Committer:
MBroek
Date:
Mon Oct 24 09:32:46 2016 +0000
Revision:
18:6cfe248b2290
Parent:
17:525b785f007a
Child:
19:35f3da6c6969
input potmeters vervangen door knoppen

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 16:5a749b319276 33 InterruptIn motor_switch_button(D8);
MBroek 16:5a749b319276 34 InterruptIn state_switch_button(D9);
MBroek 17:525b785f007a 35 DigitalIn EMG_sim1(SW2);
MBroek 17:525b785f007a 36 DigitalIn EMG_sim2(SW3);
MBroek 17:525b785f007a 37
MBroek 1:5b3fa8e47e8b 38 AnalogIn pot1(A1); // Dit is de gesimuleerde emg1
MBroek 1:5b3fa8e47e8b 39 AnalogIn pot2(A2); // Dit is de gesimuleerde emg2
MBroek 0:9e053ed05c69 40
MBroek 13:b0d3c547bf2f 41
MBroek 13:b0d3c547bf2f 42 DigitalOut ledred(LED_RED, 1);
MBroek 13:b0d3c547bf2f 43 DigitalOut ledgreen(LED_GREEN, 1);
MBroek 16:5a749b319276 44 DigitalOut ledblue(LED_BLUE, 1);
MBroek 13:b0d3c547bf2f 45
MBroek 0:9e053ed05c69 46
MBroek 4:6524b198721f 47 //Definieren van de tickers ------------------------------------------
MBroek 17:525b785f007a 48 Ticker motor_ticker; // Deze ticker activeert het motor_runs programma, dit leest motor pos en ref pos uit, rekent de PID uit en stuurt met dit de motoren
MBroek 4:6524b198721f 49 Ticker hidscope_ticker;
MBroek 4:6524b198721f 50
MBroek 4:6524b198721f 51
MBroek 0:9e053ed05c69 52
MBroek 0:9e053ed05c69 53 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
MBroek 4:6524b198721f 54
MBroek 16:5a749b319276 55 enum states_enum{off, init_m1, init_m2, finish_init_m1, finish_init_m2, 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 56 states_enum states = off;
MBroek 14:f51d51395803 57
MBroek 13:b0d3c547bf2f 58 double error_prev_1_in = 0.0, error_int_1_in = 0.0; // De error waardes voor het initialiseren
MBroek 13:b0d3c547bf2f 59 double PID_output_1_in = 0.0; // De PID output voor het initialiseren
MBroek 16:5a749b319276 60
MBroek 16:5a749b319276 61 double error_prev_2_in = 0.0, error_int_2_in = 0.0; // De error waardes voor het initialiseren
MBroek 16:5a749b319276 62 double PID_output_2_in = 0.0; // De PID output voor het initialiseren
MBroek 16:5a749b319276 63
MBroek 4:6524b198721f 64 double position_motor1;
MBroek 4:6524b198721f 65
MBroek 4:6524b198721f 66 int counts1; // Pulses van motoren
MBroek 4:6524b198721f 67 int counts2;
MBroek 0:9e053ed05c69 68
MBroek 0:9e053ed05c69 69 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 15:3f3d87513a6b 70 const double rad_per_count = 2.0*pi/8400.0; // Aantal rad per puls uit encoder
MBroek 0:9e053ed05c69 71
MBroek 11:91613b22bc00 72 const double radius_tandwiel = 1.0;
MBroek 9:f735baee0c2b 73 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 74
MBroek 4:6524b198721f 75 double error1_int = 0.00000; // Initiele error integral waardes
MBroek 4:6524b198721f 76 double error2_int = 0.00000;
MBroek 0:9e053ed05c69 77
MBroek 0:9e053ed05c69 78 const double T_getpos = 0.01; // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
MBroek 0:9e053ed05c69 79
MBroek 4:6524b198721f 80 volatile bool flag1 = false, flag2 = false; // De flags voor de functies aangeroepen door de tickers
MBroek 4:6524b198721f 81
MBroek 15:3f3d87513a6b 82 const double Kp_1 = 0.1; // De PID variablen voor motor 1
MBroek 15:3f3d87513a6b 83 const double Kd_1 = 0.01;
MBroek 15:3f3d87513a6b 84 const double Ki_1 = 0.01;
MBroek 4:6524b198721f 85
MBroek 9:f735baee0c2b 86 const double Kp_2 = 1.0000000; // De PID variablen voor motor 2
MBroek 9:f735baee0c2b 87 const double Kd_2 = 0.1;
MBroek 9:f735baee0c2b 88 const double Ki_2 = 0.3;
MBroek 5:c510ab61af28 89
MBroek 9:f735baee0c2b 90 const double vrijheid_m1_rad = 0.5*pi; // Dit is de uiterste postitie vd arm in radialen
MBroek 9:f735baee0c2b 91 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 92
MBroek 12:8aba69d8d0d0 93 const double starting_pos_m1 = 0.5*pi; // Startin posities van de motoren
MBroek 13:b0d3c547bf2f 94 const double starting_pos_m2 = 0.25;
MBroek 11:91613b22bc00 95
MBroek 13:b0d3c547bf2f 96 double reference_position_motor1 = 0; // Dit zijn de eerste posities waar de motoren heen willen bij de initialisatie
MBroek 10:755b9228cc42 97 double reference_position_motor2 = 0;
MBroek 9:f735baee0c2b 98
MBroek 13:b0d3c547bf2f 99 double PID_output_1 = 0.0; // De eerste PID_output, deze is nul anders gaan de motoren draaien
MBroek 13:b0d3c547bf2f 100 double PID_output_2 = 0.0;
MBroek 5:c510ab61af28 101
MBroek 13:b0d3c547bf2f 102 double error_prev_1 = 0.0; // De initiele previous error
MBroek 13:b0d3c547bf2f 103 double error_int_1 = 0.0; // De initiele intergral error
MBroek 10:755b9228cc42 104
MBroek 13:b0d3c547bf2f 105 double error_prev_2 = 0.0;
MBroek 13:b0d3c547bf2f 106 double error_int_2 = 0.0;
MBroek 5:c510ab61af28 107
MBroek 17:525b785f007a 108 enum which_motor{motor1, motor2}; // enum van de motoren
MBroek 15:3f3d87513a6b 109 which_motor motor_that_runs = motor1;
MBroek 15:3f3d87513a6b 110
MBroek 9:f735baee0c2b 111
MBroek 0:9e053ed05c69 112
MBroek 0:9e053ed05c69 113
MBroek 0:9e053ed05c69 114
MBroek 0:9e053ed05c69 115
MBroek 0:9e053ed05c69 116 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 0:9e053ed05c69 117
MBroek 13:b0d3c547bf2f 118
MBroek 15:3f3d87513a6b 119 // De statechanger -----------------------------------------------------------
MBroek 15:3f3d87513a6b 120 void state_changer(){
MBroek 15:3f3d87513a6b 121 if(states == off){
MBroek 15:3f3d87513a6b 122 states = init_m1;
MBroek 15:3f3d87513a6b 123 }
MBroek 16:5a749b319276 124 else if(states == init_m1){
MBroek 16:5a749b319276 125 states = finish_init_m1;
MBroek 16:5a749b319276 126 }
MBroek 16:5a749b319276 127 else if(states == finish_init_m1){
MBroek 16:5a749b319276 128 states = init_m2;
MBroek 16:5a749b319276 129 }
MBroek 16:5a749b319276 130 else if(states == init_m2){
MBroek 16:5a749b319276 131 states = finish_init_m2;
MBroek 16:5a749b319276 132 }
MBroek 16:5a749b319276 133 else if(states == finish_init_m2){
MBroek 16:5a749b319276 134 states = run;
MBroek 16:5a749b319276 135 }
MBroek 16:5a749b319276 136 else{
MBroek 16:5a749b319276 137 states = off;
MBroek 16:5a749b319276 138 }
MBroek 16:5a749b319276 139 }
MBroek 16:5a749b319276 140
MBroek 13:b0d3c547bf2f 141
MBroek 9:f735baee0c2b 142 // Functie voor het switchen tussen de motors ------------------------------
MBroek 9:f735baee0c2b 143 void motor_switch(){
MBroek 15:3f3d87513a6b 144 if(motor_that_runs == motor1){
MBroek 15:3f3d87513a6b 145 motor_that_runs = motor2;
MBroek 15:3f3d87513a6b 146 }
MBroek 15:3f3d87513a6b 147 else{
MBroek 15:3f3d87513a6b 148 motor_that_runs = motor1;
MBroek 15:3f3d87513a6b 149 }
MBroek 16:5a749b319276 150 }
MBroek 15:3f3d87513a6b 151
MBroek 9:f735baee0c2b 152
MBroek 9:f735baee0c2b 153
MBroek 1:5b3fa8e47e8b 154 // Functie voor het vinden van de positie van motor 1 ---------------------
MBroek 13:b0d3c547bf2f 155 double get_position_m1(const double radpercount){ //returned de positie van de motor in radialen (positie vd arm in radialen)
MBroek 0:9e053ed05c69 156 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 13:b0d3c547bf2f 157 return radpercount * counts1; // rekent positie motor1 uit en geeft deze als output vd functie
MBroek 1:5b3fa8e47e8b 158 }
MBroek 1:5b3fa8e47e8b 159
MBroek 1:5b3fa8e47e8b 160
MBroek 1:5b3fa8e47e8b 161 // Functie voor vinden van de positie van motor 2 -----------------------------
MBroek 13:b0d3c547bf2f 162 double get_position_m2(const double meterpercount){ // returned de positie van het karretje in meter
MBroek 1:5b3fa8e47e8b 163 counts2 = encoder_motor2.getPulses(); // leest aantal pulses vd encoder af
MBroek 13:b0d3c547bf2f 164 return meterpercount * counts2; // rekent de positie van het karretje uit en geeft dit als output vd functie
MBroek 1:5b3fa8e47e8b 165 }
MBroek 1:5b3fa8e47e8b 166
MBroek 1:5b3fa8e47e8b 167
MBroek 5:c510ab61af28 168 // Functie voor het vinden van de reference position van motor 1 --------------------
MBroek 13:b0d3c547bf2f 169 double get_reference_position_m1(const double aantal_rad){
MBroek 18:6cfe248b2290 170 double ref_pos = EMG_sim1.read() - EMG_sim2.read(); // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
MBroek 13:b0d3c547bf2f 171 return ref_pos * aantal_rad; // Aantal rad is de uiterste positie vd arm in radialen
MBroek 5:c510ab61af28 172 }
MBroek 5:c510ab61af28 173
MBroek 5:c510ab61af28 174
MBroek 5:c510ab61af28 175 // Functie voor het vinden van de reference position van motor 2 --------------------
MBroek 13:b0d3c547bf2f 176 double get_reference_position_m2(const double aantal_meter){
MBroek 18:6cfe248b2290 177 double ref_pos = EMG_sim1.read() - EMG_sim2.read(); // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
MBroek 13:b0d3c547bf2f 178 return ref_pos * aantal_meter; // Aantal meter is hoeveelheid radialen van middelpunt tot minima.
MBroek 1:5b3fa8e47e8b 179 }
MBroek 0:9e053ed05c69 180
MBroek 3:581c5918b590 181
MBroek 3:581c5918b590 182 // HIDScope functie ----------------------------------------------------------------------
MBroek 7:435f984781ab 183 void set_scope(double input1, double input2, double input3){
MBroek 7:435f984781ab 184 scope.set(0, input1);
MBroek 7:435f984781ab 185 scope.set(1, input2);
MBroek 7:435f984781ab 186 scope.set(2, input3);
MBroek 3:581c5918b590 187 scope.send();
MBroek 3:581c5918b590 188 }
MBroek 3:581c5918b590 189
MBroek 0:9e053ed05c69 190
MBroek 1:5b3fa8e47e8b 191 // De PID-controller voor de motors -------------------------------------------------
MBroek 6:e0857842e8cd 192 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 193 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 6:e0857842e8cd 194 double error_dif = (error-e_prev)/T_getpos; // De error differentieren
MBroek 1:5b3fa8e47e8b 195 //error_dif = ..... // Filter biquad poep
MBroek 6:e0857842e8cd 196 e_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 6:e0857842e8cd 197 e_int = e_int + T_getpos*error; // De error integreren
MBroek 6:e0857842e8cd 198 return Kp*error + Ki*e_int + Kd*error_dif; // De uiteindelijke PID output
MBroek 0:9e053ed05c69 199 }
MBroek 0:9e053ed05c69 200
MBroek 0:9e053ed05c69 201
MBroek 2:6bef5397e8a9 202 // Motor 1 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 203 void set_motor1(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 204 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 205 motor1_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 206 }
MBroek 2:6bef5397e8a9 207 else {
MBroek 2:6bef5397e8a9 208 motor1_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 209 }
MBroek 2:6bef5397e8a9 210 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 211 motor_input = 1;
MBroek 2:6bef5397e8a9 212 }
MBroek 2:6bef5397e8a9 213 motor1_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 214 }
MBroek 2:6bef5397e8a9 215
MBroek 2:6bef5397e8a9 216
MBroek 2:6bef5397e8a9 217 // Motor 2 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 218 void set_motor2(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 219 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 220 motor2_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 221 }
MBroek 2:6bef5397e8a9 222 else {
MBroek 2:6bef5397e8a9 223 motor2_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 224 }
MBroek 2:6bef5397e8a9 225 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 226 motor_input = 1;
MBroek 2:6bef5397e8a9 227 }
MBroek 2:6bef5397e8a9 228 motor2_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 229 }
MBroek 2:6bef5397e8a9 230
MBroek 0:9e053ed05c69 231
MBroek 4:6524b198721f 232 // De go-flag functies -----------------------------------------------------------------
MBroek 4:6524b198721f 233 void flag1_activate(){flag1=true;} // Activeert de flag
MBroek 14:f51d51395803 234 void flag2_activate(){flag2=true;}
MBroek 14:f51d51395803 235
MBroek 14:f51d51395803 236
MBroek 16:5a749b319276 237 // De initialiseren functie van motor 1 ------------------------------------------------------------
MBroek 17:525b785f007a 238 void initialise_m1(){ // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
MBroek 14:f51d51395803 239 if (flag1){
MBroek 14:f51d51395803 240 flag1 = false;
MBroek 14:f51d51395803 241 ledred = !ledred;
MBroek 16:5a749b319276 242 ledblue.write(1);
MBroek 17:525b785f007a 243 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); // PID met een vaste ref pos.
MBroek 14:f51d51395803 244 set_motor1(PID_output_1_in);
MBroek 14:f51d51395803 245 }
MBroek 14:f51d51395803 246 }
MBroek 4:6524b198721f 247
MBroek 4:6524b198721f 248
MBroek 16:5a749b319276 249 // De initialiseren functie van motor 2 ------------------------------------------------------------
MBroek 17:525b785f007a 250 void initialise_m2(){ // Hetzelfde als hierboven
MBroek 16:5a749b319276 251 if (flag1){
MBroek 16:5a749b319276 252 flag1 = false;
MBroek 16:5a749b319276 253 ledblue = !ledblue;
MBroek 16:5a749b319276 254 ledgreen.write(1);
MBroek 17:525b785f007a 255 PID_output_2_in = PID_controller(reference_position_motor2, get_position_m2(meter_per_count)+starting_pos_m2, error_prev_2_in, error_int_2_in, Kp_2, Kd_2, Ki_2); // PID met vaste ref pos.
MBroek 16:5a749b319276 256 set_motor2(PID_output_2_in);
MBroek 16:5a749b319276 257 }
MBroek 16:5a749b319276 258 }
MBroek 16:5a749b319276 259
MBroek 16:5a749b319276 260
MBroek 16:5a749b319276 261 // De functies voor het afronden van de initialisatie ----------------------------------------
MBroek 17:525b785f007a 262 void finish_initialising_m2(){ // Deze functie rond het initialiseren af, hij reset de encoder en zet de motor uit
MBroek 16:5a749b319276 263 encoder_motor2.reset();
MBroek 16:5a749b319276 264 ledblue.write(1);
MBroek 16:5a749b319276 265 ledred.write(0);
MBroek 16:5a749b319276 266 motor2_speed_pin = 0;
MBroek 16:5a749b319276 267 }
MBroek 16:5a749b319276 268
MBroek 16:5a749b319276 269
MBroek 16:5a749b319276 270 // De functies voor het afronden van de initialisatie ----------------------------------------
MBroek 17:525b785f007a 271 void finish_initialising_m1(){ // Zelfde als hierboven
MBroek 17:525b785f007a 272 encoder_motor1.reset();
MBroek 16:5a749b319276 273 ledred.write(1);
MBroek 16:5a749b319276 274 ledgreen.write(0);
MBroek 16:5a749b319276 275 motor1_speed_pin = 0;
MBroek 16:5a749b319276 276 }
MBroek 16:5a749b319276 277
MBroek 16:5a749b319276 278
MBroek 16:5a749b319276 279
MBroek 15:3f3d87513a6b 280 // De functie die de motoren aanstuurt -------------------------------------------------
MBroek 17:525b785f007a 281 void running_motors(){ // Deze functie kijkt welke motor moet draaien en rekent dan de PID uit en geeft dit door aan de motor input functie
MBroek 15:3f3d87513a6b 282 if (flag1){
MBroek 15:3f3d87513a6b 283 flag1 = false;
MBroek 16:5a749b319276 284 ledred.write(1);
MBroek 15:3f3d87513a6b 285 ledgreen = !ledgreen;
MBroek 15:3f3d87513a6b 286 switch (motor_that_runs){
MBroek 15:3f3d87513a6b 287 case motor1 : // In deze case draait alleen motor 1
MBroek 15:3f3d87513a6b 288 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 289 set_motor1(PID_output_1);
MBroek 15:3f3d87513a6b 290 break;
MBroek 15:3f3d87513a6b 291 case motor2 :
MBroek 15:3f3d87513a6b 292 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 293 set_motor2(PID_output_2);
MBroek 15:3f3d87513a6b 294 break;
MBroek 15:3f3d87513a6b 295 }
MBroek 15:3f3d87513a6b 296 }
MBroek 15:3f3d87513a6b 297 }
MBroek 15:3f3d87513a6b 298
MBroek 15:3f3d87513a6b 299
MBroek 16:5a749b319276 300 // Dit doet de robot als hij niets doet ------------------------------------------------------
MBroek 16:5a749b319276 301 void robot_is_off(){
MBroek 17:525b785f007a 302 ledgreen.write(1);
MBroek 17:525b785f007a 303 ledblue.write(0); // Indicator ik ben uit
MBroek 17:525b785f007a 304 motor1_speed_pin = 0; // Uitzetten vd motoren
MBroek 16:5a749b319276 305 motor2_speed_pin = 0;
MBroek 16:5a749b319276 306 }
MBroek 16:5a749b319276 307
MBroek 16:5a749b319276 308
MBroek 15:3f3d87513a6b 309 // De HIDscope debug functie ----------------------------------------------------------------
MBroek 17:525b785f007a 310 void call_HIDscope(double input_1, double input_2, double input_3){ // Deze functie roept de HIDscope aan
MBroek 15:3f3d87513a6b 311 if (flag2){
MBroek 15:3f3d87513a6b 312 flag2 = false;
MBroek 15:3f3d87513a6b 313 set_scope(input_1, input_2, input_3);
MBroek 15:3f3d87513a6b 314 }
MBroek 15:3f3d87513a6b 315 }
MBroek 15:3f3d87513a6b 316
MBroek 15:3f3d87513a6b 317
MBroek 15:3f3d87513a6b 318
MBroek 0:9e053ed05c69 319
MBroek 0:9e053ed05c69 320 // DE MAIN =================================================================================================================
MBroek 0:9e053ed05c69 321 int main()
MBroek 0:9e053ed05c69 322 {
MBroek 0:9e053ed05c69 323 pc.baud(SERIALBAUD);
MBroek 0:9e053ed05c69 324
MBroek 16:5a749b319276 325 state_switch_button.fall(&state_changer); // Switcht states
MBroek 17:525b785f007a 326 motor_switch_button.fall(&motor_switch); // Switcht motors
MBroek 16:5a749b319276 327
MBroek 17:525b785f007a 328
MBroek 13:b0d3c547bf2f 329
MBroek 17:525b785f007a 330 motor_ticker.attach(&flag1_activate,0.1); // Activeert de go-flag van motor positie
MBroek 17:525b785f007a 331 hidscope_ticker.attach(&flag2_activate,0.01); // Leest ref pos en mot pos uit, rekent PID uit en stuurt motoren aan.
MBroek 13:b0d3c547bf2f 332
MBroek 16:5a749b319276 333 while(1){
MBroek 14:f51d51395803 334 switch(states){
MBroek 16:5a749b319276 335 case off : // Dan staat het programma stil en gebeurt er niks
MBroek 16:5a749b319276 336 robot_is_off();
MBroek 14:f51d51395803 337 break;
MBroek 14:f51d51395803 338 case init_m1 : // Hier voert hij alleen het initialiseren van motor 1 uit
MBroek 15:3f3d87513a6b 339 initialise_m1();
MBroek 14:f51d51395803 340 break;
MBroek 17:525b785f007a 341 case finish_init_m1 : // Hier beeindigt hij het initialiseren
MBroek 16:5a749b319276 342 finish_initialising_m1();
MBroek 16:5a749b319276 343 break;
MBroek 16:5a749b319276 344 case init_m2 :
MBroek 17:525b785f007a 345 initialise_m2(); // Voert de initialsatie van motor 2 uit
MBroek 16:5a749b319276 346 break;
MBroek 16:5a749b319276 347 case finish_init_m2 :
MBroek 17:525b785f007a 348 finish_initialising_m2(); // beeindigt de initialisatie van motor 2
MBroek 16:5a749b319276 349 break;
MBroek 15:3f3d87513a6b 350 case run :
MBroek 17:525b785f007a 351 running_motors(); // Hier voert hij het programma voor het aansturen vd motors uit
MBroek 15:3f3d87513a6b 352 break;
MBroek 14:f51d51395803 353
MBroek 15:3f3d87513a6b 354 }
MBroek 15:3f3d87513a6b 355 call_HIDscope(PID_output_1_in, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(meter_per_count));
MBroek 15:3f3d87513a6b 356 }
MBroek 15:3f3d87513a6b 357 }
MBroek 14:f51d51395803 358
MBroek 14:f51d51395803 359
MBroek 14:f51d51395803 360
MBroek 14:f51d51395803 361
MBroek 14:f51d51395803 362
MBroek 14:f51d51395803 363
MBroek 14:f51d51395803 364
MBroek 14:f51d51395803 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
MBroek 0:9e053ed05c69 381
MBroek 0:9e053ed05c69 382
MBroek 0:9e053ed05c69 383
MBroek 0:9e053ed05c69 384
MBroek 0:9e053ed05c69 385
MBroek 0:9e053ed05c69 386
MBroek 0:9e053ed05c69 387
MBroek 0:9e053ed05c69 388
MBroek 0:9e053ed05c69 389
MBroek 0:9e053ed05c69 390
MBroek 0:9e053ed05c69 391
MBroek 0:9e053ed05c69 392
MBroek 0:9e053ed05c69 393
MBroek 0:9e053ed05c69 394
MBroek 0:9e053ed05c69 395
MBroek 0:9e053ed05c69 396
MBroek 0:9e053ed05c69 397
MBroek 0:9e053ed05c69 398
MBroek 0:9e053ed05c69 399
MBroek 0:9e053ed05c69 400
MBroek 0:9e053ed05c69 401
MBroek 0:9e053ed05c69 402
MBroek 0:9e053ed05c69 403
MBroek 0:9e053ed05c69 404
MBroek 0:9e053ed05c69 405
MBroek 0:9e053ed05c69 406
MBroek 0:9e053ed05c69 407
MBroek 0:9e053ed05c69 408
MBroek 0:9e053ed05c69 409
MBroek 0:9e053ed05c69 410
MBroek 0:9e053ed05c69 411
MBroek 0:9e053ed05c69 412
MBroek 0:9e053ed05c69 413
MBroek 0:9e053ed05c69 414
MBroek 0:9e053ed05c69 415
MBroek 0:9e053ed05c69 416
MBroek 0:9e053ed05c69 417
MBroek 0:9e053ed05c69 418
MBroek 0:9e053ed05c69 419