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@22:6dfe5554b96e, 2016-10-25 (annotated)

Committer:
MBroek
Date:
Tue Oct 25 12:03:43 2016 +0000
Revision:
22:6dfe5554b96e
Parent:
21:3db3f2d56d56
Child:
23:3902ee714281
Variabelen opgeriumd

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 19:35f3da6c6969 33 InterruptIn motor_switch_button(D3);
MBroek 19:35f3da6c6969 34 InterruptIn state_switch_button(D2);
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 21:3db3f2d56d56 55 enum states_enum{off, 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 56 states_enum states = off;
MBroek 14:f51d51395803 57
MBroek 21:3db3f2d56d56 58 double ref_pos_prev_m1 = 0.0; // De initiele ref_pos_pref
MBroek 21:3db3f2d56d56 59 double ref_pos_prev_m2 = 0.0;
MBroek 21:3db3f2d56d56 60
MBroek 4:6524b198721f 61 int counts1; // Pulses van motoren
MBroek 4:6524b198721f 62 int counts2;
MBroek 0:9e053ed05c69 63
MBroek 0:9e053ed05c69 64 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 15:3f3d87513a6b 65 const double rad_per_count = 2.0*pi/8400.0; // Aantal rad per puls uit encoder
MBroek 0:9e053ed05c69 66
MBroek 11:91613b22bc00 67 const double radius_tandwiel = 1.0;
MBroek 9:f735baee0c2b 68 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 69
MBroek 4:6524b198721f 70 double error1_int = 0.00000; // Initiele error integral waardes
MBroek 4:6524b198721f 71 double error2_int = 0.00000;
MBroek 0:9e053ed05c69 72
MBroek 21:3db3f2d56d56 73 const double T_motor_function = 0.01; // Periode van de frequentie van het aanroepen van onder andere positiechecker (get_position) en de rest vd motor functie
MBroek 0:9e053ed05c69 74
MBroek 4:6524b198721f 75 volatile bool flag1 = false, flag2 = false; // De flags voor de functies aangeroepen door de tickers
MBroek 4:6524b198721f 76
MBroek 21:3db3f2d56d56 77 const double Kp_1 = 0.5; // De PID variablen voor motor 1
MBroek 21:3db3f2d56d56 78 const double Kd_1 = 0.0;
MBroek 21:3db3f2d56d56 79 const double Ki_1 = 0.0;
MBroek 4:6524b198721f 80
MBroek 9:f735baee0c2b 81 const double Kp_2 = 1.0000000; // De PID variablen voor motor 2
MBroek 9:f735baee0c2b 82 const double Kd_2 = 0.1;
MBroek 9:f735baee0c2b 83 const double Ki_2 = 0.3;
MBroek 5:c510ab61af28 84
MBroek 9:f735baee0c2b 85 const double vrijheid_m1_rad = 0.5*pi; // Dit is de uiterste postitie vd arm in radialen
MBroek 9:f735baee0c2b 86 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 87
MBroek 21:3db3f2d56d56 88 const double initial_pos_m1 = vrijheid_m1_rad; // Startin posities van de motoren
MBroek 21:3db3f2d56d56 89 const double initial_pos_m2 = vrijheid_m2_meter;
MBroek 9:f735baee0c2b 90
MBroek 13:b0d3c547bf2f 91 double PID_output_1 = 0.0; // De eerste PID_output, deze is nul anders gaan de motoren draaien
MBroek 13:b0d3c547bf2f 92 double PID_output_2 = 0.0;
MBroek 5:c510ab61af28 93
MBroek 13:b0d3c547bf2f 94 double error_prev_1 = 0.0; // De initiele previous error
MBroek 13:b0d3c547bf2f 95 double error_int_1 = 0.0; // De initiele intergral error
MBroek 10:755b9228cc42 96
MBroek 13:b0d3c547bf2f 97 double error_prev_2 = 0.0;
MBroek 13:b0d3c547bf2f 98 double error_int_2 = 0.0;
MBroek 5:c510ab61af28 99
MBroek 20:2fdb069ffcae 100 double reference_pos_m2; // De reference positie waar de motor heen wil afhankelijk van het EMG signaal
MBroek 20:2fdb069ffcae 101 double reference_pos_m1;
MBroek 20:2fdb069ffcae 102
MBroek 17:525b785f007a 103 enum which_motor{motor1, motor2}; // enum van de motoren
MBroek 15:3f3d87513a6b 104 which_motor motor_that_runs = motor1;
MBroek 15:3f3d87513a6b 105
MBroek 9:f735baee0c2b 106
MBroek 0:9e053ed05c69 107
MBroek 0:9e053ed05c69 108
MBroek 0:9e053ed05c69 109
MBroek 0:9e053ed05c69 110
MBroek 0:9e053ed05c69 111 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 0:9e053ed05c69 112
MBroek 13:b0d3c547bf2f 113
MBroek 15:3f3d87513a6b 114 // De statechanger -----------------------------------------------------------
MBroek 15:3f3d87513a6b 115 void state_changer(){
MBroek 15:3f3d87513a6b 116 if(states == off){
MBroek 21:3db3f2d56d56 117 states = init;
MBroek 16:5a749b319276 118 }
MBroek 21:3db3f2d56d56 119 else if(states == init){
MBroek 16:5a749b319276 120 states = run;
MBroek 16:5a749b319276 121 }
MBroek 16:5a749b319276 122 else{
MBroek 16:5a749b319276 123 states = off;
MBroek 16:5a749b319276 124 }
MBroek 16:5a749b319276 125 }
MBroek 16:5a749b319276 126
MBroek 13:b0d3c547bf2f 127
MBroek 9:f735baee0c2b 128 // Functie voor het switchen tussen de motors ------------------------------
MBroek 9:f735baee0c2b 129 void motor_switch(){
MBroek 15:3f3d87513a6b 130 if(motor_that_runs == motor1){
MBroek 15:3f3d87513a6b 131 motor_that_runs = motor2;
MBroek 15:3f3d87513a6b 132 }
MBroek 15:3f3d87513a6b 133 else{
MBroek 15:3f3d87513a6b 134 motor_that_runs = motor1;
MBroek 15:3f3d87513a6b 135 }
MBroek 16:5a749b319276 136 }
MBroek 15:3f3d87513a6b 137
MBroek 9:f735baee0c2b 138
MBroek 9:f735baee0c2b 139
MBroek 1:5b3fa8e47e8b 140 // Functie voor het vinden van de positie van motor 1 ---------------------
MBroek 13:b0d3c547bf2f 141 double get_position_m1(const double radpercount){ //returned de positie van de motor in radialen (positie vd arm in radialen)
MBroek 0:9e053ed05c69 142 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 21:3db3f2d56d56 143 return radpercount * counts1 + initial_pos_m1; // rekent positie motor1 uit en geeft deze als output vd functie
MBroek 1:5b3fa8e47e8b 144 }
MBroek 1:5b3fa8e47e8b 145
MBroek 1:5b3fa8e47e8b 146
MBroek 1:5b3fa8e47e8b 147 // Functie voor vinden van de positie van motor 2 -----------------------------
MBroek 13:b0d3c547bf2f 148 double get_position_m2(const double meterpercount){ // returned de positie van het karretje in meter
MBroek 1:5b3fa8e47e8b 149 counts2 = encoder_motor2.getPulses(); // leest aantal pulses vd encoder af
MBroek 13:b0d3c547bf2f 150 return meterpercount * counts2; // rekent de positie van het karretje uit en geeft dit als output vd functie
MBroek 1:5b3fa8e47e8b 151 }
MBroek 1:5b3fa8e47e8b 152
MBroek 1:5b3fa8e47e8b 153
MBroek 5:c510ab61af28 154 // Functie voor het vinden van de reference position van motor 1 --------------------
MBroek 21:3db3f2d56d56 155 double get_reference_position_m1(double &ref_pos_prev, const double vrijheid_rad){
MBroek 22:6dfe5554b96e 156 double ref_pos;
MBroek 21:3db3f2d56d56 157 int final_signal = EMG_sim1.read() - EMG_sim2.read(); // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
MBroek 21:3db3f2d56d56 158 switch(final_signal){
MBroek 21:3db3f2d56d56 159 case 0 :
MBroek 22:6dfe5554b96e 160 ref_pos = ref_pos_prev;
MBroek 21:3db3f2d56d56 161 break;
MBroek 21:3db3f2d56d56 162 case 1 :
MBroek 22:6dfe5554b96e 163 ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad;
MBroek 21:3db3f2d56d56 164 break;
MBroek 21:3db3f2d56d56 165 case -1 :
MBroek 22:6dfe5554b96e 166 ref_pos = ref_pos_prev - T_motor_function*vrijheid_rad;
MBroek 21:3db3f2d56d56 167 break;
MBroek 21:3db3f2d56d56 168 }
MBroek 22:6dfe5554b96e 169 if(ref_pos >= vrijheid_rad){
MBroek 22:6dfe5554b96e 170 ref_pos = vrijheid_rad;
MBroek 21:3db3f2d56d56 171 }
MBroek 22:6dfe5554b96e 172 if(ref_pos <= -vrijheid_rad){
MBroek 22:6dfe5554b96e 173 ref_pos = -vrijheid_rad;
MBroek 21:3db3f2d56d56 174 }
MBroek 22:6dfe5554b96e 175 ref_pos_prev = ref_pos;
MBroek 21:3db3f2d56d56 176 return ref_pos_prev; // vrijheid_rad is de uiterste positie vd arm in radialen
MBroek 5:c510ab61af28 177 }
MBroek 5:c510ab61af28 178
MBroek 5:c510ab61af28 179
MBroek 5:c510ab61af28 180 // Functie voor het vinden van de reference position van motor 2 --------------------
MBroek 13:b0d3c547bf2f 181 double get_reference_position_m2(const double aantal_meter){
MBroek 21:3db3f2d56d56 182 double final_signal = 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 21:3db3f2d56d56 183 return final_signal * aantal_meter; // Aantal meter is hoeveelheid radialen van middelpunt tot minima.
MBroek 1:5b3fa8e47e8b 184 }
MBroek 0:9e053ed05c69 185
MBroek 3:581c5918b590 186
MBroek 3:581c5918b590 187 // HIDScope functie ----------------------------------------------------------------------
MBroek 7:435f984781ab 188 void set_scope(double input1, double input2, double input3){
MBroek 7:435f984781ab 189 scope.set(0, input1);
MBroek 7:435f984781ab 190 scope.set(1, input2);
MBroek 7:435f984781ab 191 scope.set(2, input3);
MBroek 3:581c5918b590 192 scope.send();
MBroek 3:581c5918b590 193 }
MBroek 3:581c5918b590 194
MBroek 0:9e053ed05c69 195
MBroek 1:5b3fa8e47e8b 196 // De PID-controller voor de motors -------------------------------------------------
MBroek 6:e0857842e8cd 197 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 198 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 21:3db3f2d56d56 199 double error_dif = (error-e_prev)/T_motor_function; // De error differentieren
MBroek 1:5b3fa8e47e8b 200 //error_dif = ..... // Filter biquad poep
MBroek 6:e0857842e8cd 201 e_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 21:3db3f2d56d56 202 e_int = e_int + T_motor_function*error; // De error integreren
MBroek 6:e0857842e8cd 203 return Kp*error + Ki*e_int + Kd*error_dif; // De uiteindelijke PID output
MBroek 0:9e053ed05c69 204 }
MBroek 0:9e053ed05c69 205
MBroek 0:9e053ed05c69 206
MBroek 2:6bef5397e8a9 207 // Motor 1 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 208 void set_motor1(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 209 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 210 motor1_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 211 }
MBroek 2:6bef5397e8a9 212 else {
MBroek 2:6bef5397e8a9 213 motor1_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 214 }
MBroek 2:6bef5397e8a9 215 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 216 motor_input = 1;
MBroek 2:6bef5397e8a9 217 }
MBroek 2:6bef5397e8a9 218 motor1_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 219 }
MBroek 2:6bef5397e8a9 220
MBroek 2:6bef5397e8a9 221
MBroek 2:6bef5397e8a9 222 // Motor 2 besturen ---------------------------------------------------------------------
MBroek 2:6bef5397e8a9 223 void set_motor2(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 2:6bef5397e8a9 224 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 2:6bef5397e8a9 225 motor2_direction_pin = 1; // Clockwise
MBroek 2:6bef5397e8a9 226 }
MBroek 2:6bef5397e8a9 227 else {
MBroek 2:6bef5397e8a9 228 motor2_direction_pin = 0; // CounterClockwise
MBroek 2:6bef5397e8a9 229 }
MBroek 2:6bef5397e8a9 230 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 2:6bef5397e8a9 231 motor_input = 1;
MBroek 2:6bef5397e8a9 232 }
MBroek 2:6bef5397e8a9 233 motor2_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 2:6bef5397e8a9 234 }
MBroek 2:6bef5397e8a9 235
MBroek 0:9e053ed05c69 236
MBroek 4:6524b198721f 237 // De go-flag functies -----------------------------------------------------------------
MBroek 4:6524b198721f 238 void flag1_activate(){flag1=true;} // Activeert de flag
MBroek 14:f51d51395803 239 void flag2_activate(){flag2=true;}
MBroek 14:f51d51395803 240
MBroek 14:f51d51395803 241
MBroek 21:3db3f2d56d56 242 // Dit doet de robot als hij niets doet ------------------------------------------------------
MBroek 21:3db3f2d56d56 243 void robot_is_off(){
MBroek 21:3db3f2d56d56 244 ledgreen.write(1);
MBroek 21:3db3f2d56d56 245 ledred.write(0); // Indicator ik ben uit
MBroek 21:3db3f2d56d56 246 motor1_speed_pin = 0; // Uitzetten vd motoren
MBroek 16:5a749b319276 247 motor2_speed_pin = 0;
MBroek 16:5a749b319276 248 }
MBroek 16:5a749b319276 249
MBroek 16:5a749b319276 250
MBroek 21:3db3f2d56d56 251 // De initialiseren functie ------------------------------------------------------------
MBroek 21:3db3f2d56d56 252 void initialise(){ // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
MBroek 21:3db3f2d56d56 253 ref_pos_prev_m1 = initial_pos_m1;
MBroek 21:3db3f2d56d56 254 ref_pos_prev_m2 = initial_pos_m2;
MBroek 16:5a749b319276 255 ledgreen.write(0);
MBroek 21:3db3f2d56d56 256 encoder_motor1.reset();
MBroek 21:3db3f2d56d56 257 encoder_motor2.reset();
MBroek 16:5a749b319276 258 motor1_speed_pin = 0;
MBroek 21:3db3f2d56d56 259 }
MBroek 16:5a749b319276 260
MBroek 16:5a749b319276 261
MBroek 15:3f3d87513a6b 262 // De functie die de motoren aanstuurt -------------------------------------------------
MBroek 17:525b785f007a 263 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 264 if (flag1){
MBroek 15:3f3d87513a6b 265 flag1 = false;
MBroek 16:5a749b319276 266 ledred.write(1);
MBroek 15:3f3d87513a6b 267 switch (motor_that_runs){
MBroek 15:3f3d87513a6b 268 case motor1 : // In deze case draait alleen motor 1
MBroek 21:3db3f2d56d56 269 reference_pos_m1 = get_reference_position_m1(ref_pos_prev_m1, vrijheid_m1_rad);
MBroek 20:2fdb069ffcae 270 PID_output_1 = PID_controller(reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
MBroek 20:2fdb069ffcae 271 PID_output_2 = PID_controller(-reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt er voor dat motor2 meedraait met motor1
MBroek 15:3f3d87513a6b 272 set_motor1(PID_output_1);
MBroek 20:2fdb069ffcae 273 set_motor2(PID_output_2);
MBroek 15:3f3d87513a6b 274 break;
MBroek 15:3f3d87513a6b 275 case motor2 :
MBroek 20:2fdb069ffcae 276 reference_pos_m2 = get_reference_position_m2(vrijheid_m2_meter);
MBroek 20:2fdb069ffcae 277 PID_output_2 = PID_controller(reference_pos_m2, get_position_m2(meter_per_count), error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
MBroek 20:2fdb069ffcae 278 PID_output_1 = PID_controller(reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt ervoor dat motor1 op de dezelfde positie blijft als motor 2 draait.
MBroek 15:3f3d87513a6b 279 set_motor2(PID_output_2);
MBroek 20:2fdb069ffcae 280 set_motor1(PID_output_1);
MBroek 15:3f3d87513a6b 281 break;
MBroek 15:3f3d87513a6b 282 }
MBroek 15:3f3d87513a6b 283 }
MBroek 15:3f3d87513a6b 284 }
MBroek 15:3f3d87513a6b 285
MBroek 15:3f3d87513a6b 286
MBroek 15:3f3d87513a6b 287 // De HIDscope debug functie ----------------------------------------------------------------
MBroek 17:525b785f007a 288 void call_HIDscope(double input_1, double input_2, double input_3){ // Deze functie roept de HIDscope aan
MBroek 15:3f3d87513a6b 289 if (flag2){
MBroek 15:3f3d87513a6b 290 flag2 = false;
MBroek 15:3f3d87513a6b 291 set_scope(input_1, input_2, input_3);
MBroek 15:3f3d87513a6b 292 }
MBroek 15:3f3d87513a6b 293 }
MBroek 15:3f3d87513a6b 294
MBroek 15:3f3d87513a6b 295
MBroek 15:3f3d87513a6b 296
MBroek 0:9e053ed05c69 297
MBroek 0:9e053ed05c69 298 // DE MAIN =================================================================================================================
MBroek 0:9e053ed05c69 299 int main()
MBroek 0:9e053ed05c69 300 {
MBroek 0:9e053ed05c69 301 pc.baud(SERIALBAUD);
MBroek 0:9e053ed05c69 302
MBroek 16:5a749b319276 303 state_switch_button.fall(&state_changer); // Switcht states
MBroek 17:525b785f007a 304 motor_switch_button.fall(&motor_switch); // Switcht motors
MBroek 16:5a749b319276 305
MBroek 17:525b785f007a 306
MBroek 13:b0d3c547bf2f 307
MBroek 21:3db3f2d56d56 308 motor_ticker.attach(&flag1_activate, T_motor_function); // Activeert de go-flag van motor positie
MBroek 17:525b785f007a 309 hidscope_ticker.attach(&flag2_activate,0.01); // Leest ref pos en mot pos uit, rekent PID uit en stuurt motoren aan.
MBroek 13:b0d3c547bf2f 310
MBroek 16:5a749b319276 311 while(1){
MBroek 14:f51d51395803 312 switch(states){
MBroek 16:5a749b319276 313 case off : // Dan staat het programma stil en gebeurt er niks
MBroek 16:5a749b319276 314 robot_is_off();
MBroek 14:f51d51395803 315 break;
MBroek 21:3db3f2d56d56 316 case init : // Hier voert hij alleen het initialiseren van motor 1 uit
MBroek 21:3db3f2d56d56 317 initialise();
MBroek 16:5a749b319276 318 break;
MBroek 15:3f3d87513a6b 319 case run :
MBroek 17:525b785f007a 320 running_motors(); // Hier voert hij het programma voor het aansturen vd motors uit
MBroek 15:3f3d87513a6b 321 break;
MBroek 14:f51d51395803 322
MBroek 15:3f3d87513a6b 323 }
MBroek 21:3db3f2d56d56 324 call_HIDscope(PID_output_1, reference_pos_m1, get_position_m1(meter_per_count));
MBroek 15:3f3d87513a6b 325 }
MBroek 15:3f3d87513a6b 326 }
MBroek 14:f51d51395803 327
MBroek 14:f51d51395803 328
MBroek 14:f51d51395803 329
MBroek 14:f51d51395803 330
MBroek 14:f51d51395803 331
MBroek 14:f51d51395803 332
MBroek 14:f51d51395803 333
MBroek 14:f51d51395803 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
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