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.orig@30:457e42514c47, 2016-10-28 (annotated)

Committer:
MBroek
Date:
Fri Oct 28 09:19:48 2016 +0000
Revision:
30:457e42514c47
Parent:
28:97b9e50c1180 
robot testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MBroek 28:97b9e50c1180 1
MBroek 28:97b9e50c1180 2
MBroek 28:97b9e50c1180 3 // HET DEFINIEREN VAN ALLES ==========================================================================================
MBroek 28:97b9e50c1180 4
MBroek 28:97b9e50c1180 5 // Includen van alle libraries ---------------------------------------
MBroek 28:97b9e50c1180 6 #include "mbed.h"
MBroek 28:97b9e50c1180 7 #include "MODSERIAL.h"
MBroek 28:97b9e50c1180 8 #include "QEI.h"
MBroek 28:97b9e50c1180 9 #include "math.h"
MBroek 28:97b9e50c1180 10 #include "HIDScope.h"
MBroek 28:97b9e50c1180 11
MBroek 28:97b9e50c1180 12 // Definieren van de HIDscope ----------------------------------------
MBroek 28:97b9e50c1180 13 HIDScope scope(3);
MBroek 28:97b9e50c1180 14
MBroek 28:97b9e50c1180 15
MBroek 28:97b9e50c1180 16 // Definieren van de Serial en Encoder -------------------------------
MBroek 28:97b9e50c1180 17 MODSERIAL pc(USBTX, USBRX);
MBroek 28:97b9e50c1180 18 #define SERIALBAUD 115200
MBroek 28:97b9e50c1180 19
MBroek 28:97b9e50c1180 20 QEI encoder_motor1(D10,D11,NC,8400, QEI::X4_ENCODING);
MBroek 28:97b9e50c1180 21 QEI encoder_motor2(D12,D13,NC,8400, QEI::X4_ENCODING);
MBroek 28:97b9e50c1180 22
MBroek 28:97b9e50c1180 23
MBroek 28:97b9e50c1180 24 // Definieren van de Motorpins ---------------------------------------
MBroek 28:97b9e50c1180 25 DigitalOut motor1_direction_pin(D7);
MBroek 28:97b9e50c1180 26 PwmOut motor1_speed_pin(D6);
MBroek 28:97b9e50c1180 27
MBroek 28:97b9e50c1180 28 DigitalOut motor2_direction_pin(D4);
MBroek 28:97b9e50c1180 29 PwmOut motor2_speed_pin(D5);
MBroek 28:97b9e50c1180 30
MBroek 28:97b9e50c1180 31
MBroek 28:97b9e50c1180 32 //Definieren van bord-elementen --------------------------------------
MBroek 28:97b9e50c1180 33 InterruptIn motor_switch_button(D3);
MBroek 28:97b9e50c1180 34 InterruptIn state_switch_button(D2);
MBroek 28:97b9e50c1180 35 DigitalIn EMG_sim1(SW2);
MBroek 28:97b9e50c1180 36 DigitalIn EMG_sim2(SW3);
MBroek 28:97b9e50c1180 37
MBroek 28:97b9e50c1180 38 AnalogIn pot1(A1); // Dit is de gesimuleerde emg1
MBroek 28:97b9e50c1180 39 AnalogIn pot2(A2); // Dit is de gesimuleerde emg2
MBroek 28:97b9e50c1180 40
MBroek 28:97b9e50c1180 41
MBroek 28:97b9e50c1180 42 DigitalOut ledred(LED_RED, 1);
MBroek 28:97b9e50c1180 43 DigitalOut ledgreen(LED_GREEN, 1);
MBroek 28:97b9e50c1180 44 DigitalOut ledblue(LED_BLUE, 1);
MBroek 28:97b9e50c1180 45
MBroek 28:97b9e50c1180 46
MBroek 28:97b9e50c1180 47 //Definieren van de tickers ------------------------------------------
MBroek 28:97b9e50c1180 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 28:97b9e50c1180 49 Ticker hidscope_ticker;
MBroek 28:97b9e50c1180 50
MBroek 28:97b9e50c1180 51
MBroek 28:97b9e50c1180 52
MBroek 28:97b9e50c1180 53 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
MBroek 28:97b9e50c1180 54
MBroek 28:97b9e50c1180 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 28:97b9e50c1180 56 states_enum states = off;
MBroek 28:97b9e50c1180 57
MBroek 28:97b9e50c1180 58 double ref_pos_prev_m1 = 0.0; // De initiele ref_pos_pref
MBroek 28:97b9e50c1180 59 double ref_pos_prev_m2 = 0.0;
MBroek 28:97b9e50c1180 60
MBroek 28:97b9e50c1180 61 int counts1; // Pulses van motoren
MBroek 28:97b9e50c1180 62 int counts2;
MBroek 28:97b9e50c1180 63
MBroek 28:97b9e50c1180 64 const double pi = 3.14159265358979323846264338327950288419716939937510; // pi
MBroek 28:97b9e50c1180 65 const double rad_per_count = (2.0*pi)/8400.0; // Aantal rad per puls uit encoder
MBroek 28:97b9e50c1180 66
MBroek 28:97b9e50c1180 67 const double radius_tandwiel = 0.008;
MBroek 28:97b9e50c1180 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 28:97b9e50c1180 69
MBroek 28:97b9e50c1180 70 const double v_max_karretje = 8.4*radius_tandwiel; // Maximale snelheid karretje, gelimiteerd door de v_max vd motor
MBroek 28:97b9e50c1180 71
MBroek 28:97b9e50c1180 72 double error1_int = 0.00000; // Initiele error integral waardes
MBroek 28:97b9e50c1180 73 double error2_int = 0.00000;
MBroek 28:97b9e50c1180 74
MBroek 28:97b9e50c1180 75 const double T_motor_function = 0.1; // Periode van de frequentie van het aanroepen van onder andere positiechecker (get_position) en de rest vd motor functie
MBroek 28:97b9e50c1180 76
MBroek 28:97b9e50c1180 77 volatile bool flag1 = false, flag2 = false; // De flags voor de functies aangeroepen door de tickers
MBroek 28:97b9e50c1180 78
MBroek 28:97b9e50c1180 79 const double Kp_1 = 0.5; // De PID variablen voor motor 1
MBroek 28:97b9e50c1180 80 const double Kd_1 = 0.1;
MBroek 28:97b9e50c1180 81 const double Ki_1 = 0.1;
MBroek 28:97b9e50c1180 82
MBroek 28:97b9e50c1180 83 const double Kp_2 = 1.0; // De PID variablen voor motor 2
MBroek 28:97b9e50c1180 84 const double Kd_2 = 0.0;
MBroek 28:97b9e50c1180 85 const double Ki_2 = 0.0;
MBroek 28:97b9e50c1180 86
MBroek 28:97b9e50c1180 87 const double vrijheid_m1_rad = 0.5*pi; // Dit is de uiterste postitie vd arm in radialen
MBroek 28:97b9e50c1180 88 const double vrijheid_m2_meter = 0.5; // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
MBroek 28:97b9e50c1180 89
MBroek 28:97b9e50c1180 90 const double initial_pos_m1 = vrijheid_m1_rad; // Startin posities van de motoren
MBroek 28:97b9e50c1180 91 const double initial_pos_m2 = 0;
MBroek 28:97b9e50c1180 92
MBroek 28:97b9e50c1180 93 double position_motor2;
MBroek 28:97b9e50c1180 94 double position_motor1;
MBroek 28:97b9e50c1180 95 double position_motor2_prev = initial_pos_m2;
MBroek 28:97b9e50c1180 96
MBroek 28:97b9e50c1180 97 double PID_output_1 = 0.0; // De eerste PID_output, deze is nul anders gaan de motoren draaien
MBroek 28:97b9e50c1180 98 double PID_output_2 = 0.0;
MBroek 28:97b9e50c1180 99 double PID_output_2_1 = 0.0;
MBroek 28:97b9e50c1180 100
MBroek 28:97b9e50c1180 101 double error_prev_1 = 0.0; // De initiele previous error
MBroek 28:97b9e50c1180 102 double error_int_1 = 0.0; // De initiele intergral error
MBroek 28:97b9e50c1180 103
MBroek 28:97b9e50c1180 104 double error_prev_2 = 0.0;
MBroek 28:97b9e50c1180 105 double error_int_2 = 0.0;
MBroek 28:97b9e50c1180 106
MBroek 28:97b9e50c1180 107 double reference_pos_m2; // De reference positie waar de motor heen wil afhankelijk van het EMG signaal
MBroek 28:97b9e50c1180 108 double reference_pos_m1;
MBroek 28:97b9e50c1180 109
MBroek 28:97b9e50c1180 110 enum which_motor{motor1, motor2}; // enum van de motoren
MBroek 28:97b9e50c1180 111 which_motor motor_that_runs = motor1;
MBroek 28:97b9e50c1180 112
MBroek 28:97b9e50c1180 113
MBroek 28:97b9e50c1180 114
MBroek 28:97b9e50c1180 115
MBroek 28:97b9e50c1180 116
MBroek 28:97b9e50c1180 117
MBroek 28:97b9e50c1180 118 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
MBroek 28:97b9e50c1180 119
MBroek 28:97b9e50c1180 120
MBroek 28:97b9e50c1180 121 // De statechanger -----------------------------------------------------------
MBroek 28:97b9e50c1180 122 void state_changer(){
MBroek 28:97b9e50c1180 123 if(states == off){
MBroek 28:97b9e50c1180 124 states = init;
MBroek 28:97b9e50c1180 125 }
MBroek 28:97b9e50c1180 126 else if(states == init){
MBroek 28:97b9e50c1180 127 states = run;
MBroek 28:97b9e50c1180 128 }
MBroek 28:97b9e50c1180 129 else{
MBroek 28:97b9e50c1180 130 states = off;
MBroek 28:97b9e50c1180 131 }
MBroek 28:97b9e50c1180 132 }
MBroek 28:97b9e50c1180 133
MBroek 28:97b9e50c1180 134
MBroek 28:97b9e50c1180 135 // Functie voor het switchen tussen de motors ------------------------------
MBroek 28:97b9e50c1180 136 void motor_switch(){
MBroek 28:97b9e50c1180 137 if(motor_that_runs == motor1){
MBroek 28:97b9e50c1180 138 motor_that_runs = motor2;
MBroek 28:97b9e50c1180 139 }
MBroek 28:97b9e50c1180 140 else{
MBroek 28:97b9e50c1180 141 motor_that_runs = motor1;
MBroek 28:97b9e50c1180 142 }
MBroek 28:97b9e50c1180 143 }
MBroek 28:97b9e50c1180 144
MBroek 28:97b9e50c1180 145
MBroek 28:97b9e50c1180 146
MBroek 28:97b9e50c1180 147 // Functie voor het vinden van de positie van motor 1 ---------------------
MBroek 28:97b9e50c1180 148 double get_position_m1(const double radpercount){ //returned de positie van de motor in radialen (positie vd arm in radialen)
MBroek 28:97b9e50c1180 149 counts1 = encoder_motor1.getPulses(); // Leest aantal pulses uit encoder af
MBroek 28:97b9e50c1180 150 return radpercount * counts1 + initial_pos_m1; // rekent positie motor1 uit en geeft deze als output vd functie
MBroek 28:97b9e50c1180 151 }
MBroek 28:97b9e50c1180 152
MBroek 28:97b9e50c1180 153
MBroek 28:97b9e50c1180 154 // Functie voor vinden van de positie van motor 2 -----------------------------
MBroek 28:97b9e50c1180 155 double get_position_m2(const double meterpercount, double mot_pos_prev){ // returned de positie van het karretje in meter
MBroek 28:97b9e50c1180 156 double mot_pos;
MBroek 28:97b9e50c1180 157 counts2 = encoder_motor2.getPulses(); // leest aantal pulses vd encoder af
MBroek 28:97b9e50c1180 158 mot_pos = meterpercount * counts2 + mot_pos_prev;
MBroek 28:97b9e50c1180 159 encoder_motor2.reset();
MBroek 28:97b9e50c1180 160 mot_pos_prev = mot_pos;
MBroek 28:97b9e50c1180 161 return mot_pos_prev; // rekent de positie van het karretje uit en geeft dit als output vd functie
MBroek 28:97b9e50c1180 162 }
MBroek 28:97b9e50c1180 163
MBroek 28:97b9e50c1180 164
MBroek 28:97b9e50c1180 165 // Functie voor het vinden van de reference position van motor 1 --------------------
MBroek 28:97b9e50c1180 166 double get_reference_position_m1(double &ref_pos_prev, const double vrijheid_rad){
MBroek 28:97b9e50c1180 167 double ref_pos;
MBroek 28:97b9e50c1180 168 int final_signal = EMG_sim1.read() - EMG_sim2.read(); // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
MBroek 28:97b9e50c1180 169 switch(final_signal){
MBroek 28:97b9e50c1180 170 case 0 :
MBroek 28:97b9e50c1180 171 ref_pos = ref_pos_prev;
MBroek 28:97b9e50c1180 172 break;
MBroek 28:97b9e50c1180 173 case 1 :
MBroek 28:97b9e50c1180 174 ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad; // Dit dwingt af dat de ref pos veranderd met ong 0.5 rad/s
MBroek 28:97b9e50c1180 175 break;
MBroek 28:97b9e50c1180 176 case -1 :
MBroek 28:97b9e50c1180 177 ref_pos = ref_pos_prev - T_motor_function*vrijheid_rad;
MBroek 28:97b9e50c1180 178 break;
MBroek 28:97b9e50c1180 179 }
MBroek 28:97b9e50c1180 180 if(ref_pos >= vrijheid_rad){
MBroek 28:97b9e50c1180 181 ref_pos = vrijheid_rad;
MBroek 28:97b9e50c1180 182 }
MBroek 28:97b9e50c1180 183 if(ref_pos <= -vrijheid_rad){
MBroek 28:97b9e50c1180 184 ref_pos = -vrijheid_rad;
MBroek 28:97b9e50c1180 185 }
MBroek 28:97b9e50c1180 186 ref_pos_prev = ref_pos;
MBroek 28:97b9e50c1180 187 return ref_pos_prev; // vrijheid_rad is de uiterste positie vd arm in radialen
MBroek 28:97b9e50c1180 188 }
MBroek 28:97b9e50c1180 189
MBroek 28:97b9e50c1180 190
MBroek 28:97b9e50c1180 191 // Functie voor het vinden van de reference position van motor 2 --------------------
MBroek 28:97b9e50c1180 192 double get_reference_position_m2(double &ref_pos_prev, const double vrijheid_meter){
MBroek 28:97b9e50c1180 193 double ref_pos;
MBroek 28:97b9e50c1180 194 int final_signal = EMG_sim1.read() - EMG_sim2.read(); // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
MBroek 28:97b9e50c1180 195 switch(final_signal){
MBroek 28:97b9e50c1180 196 case 0 :
MBroek 28:97b9e50c1180 197 ref_pos = ref_pos_prev;
MBroek 28:97b9e50c1180 198 break;
MBroek 28:97b9e50c1180 199 case 1 :
MBroek 28:97b9e50c1180 200 ref_pos = ref_pos_prev + T_motor_function*v_max_karretje; // Hierdoor veranderd de ref_pos met de maximale snelheid vd motor (karretje)
MBroek 28:97b9e50c1180 201 break;
MBroek 28:97b9e50c1180 202 case -1 :
MBroek 28:97b9e50c1180 203 ref_pos = ref_pos_prev - T_motor_function*v_max_karretje;
MBroek 28:97b9e50c1180 204 break;
MBroek 28:97b9e50c1180 205 }
MBroek 28:97b9e50c1180 206 if(ref_pos >= vrijheid_meter){
MBroek 28:97b9e50c1180 207 ref_pos = vrijheid_meter;
MBroek 28:97b9e50c1180 208 }
MBroek 28:97b9e50c1180 209 if(ref_pos <= 0){
MBroek 28:97b9e50c1180 210 ref_pos = 0;
MBroek 28:97b9e50c1180 211 }
MBroek 28:97b9e50c1180 212 ref_pos_prev = ref_pos;
MBroek 28:97b9e50c1180 213 return ref_pos_prev;
MBroek 28:97b9e50c1180 214 }
MBroek 28:97b9e50c1180 215
MBroek 28:97b9e50c1180 216
MBroek 28:97b9e50c1180 217 // HIDScope functie ----------------------------------------------------------------------
MBroek 28:97b9e50c1180 218 void set_scope(double input1, double input2, double input3){
MBroek 28:97b9e50c1180 219 scope.set(0, input1);
MBroek 28:97b9e50c1180 220 scope.set(1, input2);
MBroek 28:97b9e50c1180 221 scope.set(2, input3);
MBroek 28:97b9e50c1180 222 scope.send();
MBroek 28:97b9e50c1180 223 }
MBroek 28:97b9e50c1180 224
MBroek 28:97b9e50c1180 225
MBroek 28:97b9e50c1180 226 // De PID-controller voor de motors -------------------------------------------------
MBroek 28:97b9e50c1180 227 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 28:97b9e50c1180 228 double error = ref_pos - mot_pos; // error uitrekenen
MBroek 28:97b9e50c1180 229 double error_dif = (error-e_prev)/T_motor_function; // De error differentieren
MBroek 28:97b9e50c1180 230 //error_dif = ..... // Filter biquad poep
MBroek 28:97b9e50c1180 231 e_prev = error; // Hier wordt de error opgeslagen voor de volgende keer
MBroek 28:97b9e50c1180 232 e_int = e_int + T_motor_function*error; // De error integreren
MBroek 28:97b9e50c1180 233 return Kp*error + Ki*e_int + Kd*error_dif; // De uiteindelijke PID output
MBroek 28:97b9e50c1180 234 }
MBroek 28:97b9e50c1180 235
MBroek 28:97b9e50c1180 236
MBroek 28:97b9e50c1180 237 // Motor 1 besturen ---------------------------------------------------------------------
MBroek 28:97b9e50c1180 238 void set_motor1(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 28:97b9e50c1180 239 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 28:97b9e50c1180 240 motor1_direction_pin = 1; // Clockwise
MBroek 28:97b9e50c1180 241 }
MBroek 28:97b9e50c1180 242 else {
MBroek 28:97b9e50c1180 243 motor1_direction_pin = 0; // CounterClockwise
MBroek 28:97b9e50c1180 244 }
MBroek 28:97b9e50c1180 245 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 28:97b9e50c1180 246 motor_input = 1;
MBroek 28:97b9e50c1180 247 }
MBroek 28:97b9e50c1180 248 motor1_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 28:97b9e50c1180 249 }
MBroek 28:97b9e50c1180 250
MBroek 28:97b9e50c1180 251
MBroek 28:97b9e50c1180 252 // Motor 2 besturen ---------------------------------------------------------------------
MBroek 28:97b9e50c1180 253 void set_motor2(double motor_input){ // functie die de motor aanstuurt mt als input de PID-output
MBroek 28:97b9e50c1180 254 if (motor_input >= 0){ // Dit checkt welke kant de motor op moet draaien
MBroek 28:97b9e50c1180 255 motor2_direction_pin = 1; // Clockwise
MBroek 28:97b9e50c1180 256 }
MBroek 28:97b9e50c1180 257 else {
MBroek 28:97b9e50c1180 258 motor2_direction_pin = 0; // CounterClockwise
MBroek 28:97b9e50c1180 259 }
MBroek 28:97b9e50c1180 260 if (fabs(motor_input)>1){ // Dit fixed de PwmOutput op maximaal 1
MBroek 28:97b9e50c1180 261 motor_input = 1;
MBroek 28:97b9e50c1180 262 }
MBroek 28:97b9e50c1180 263 motor2_speed_pin = fabs(motor_input); // Dit geeft de uiteindelijke input door aan de motor
MBroek 28:97b9e50c1180 264 }
MBroek 28:97b9e50c1180 265
MBroek 28:97b9e50c1180 266
MBroek 28:97b9e50c1180 267 // De go-flag functies -----------------------------------------------------------------
MBroek 28:97b9e50c1180 268 void flag1_activate(){flag1=true;} // Activeert de flag
MBroek 28:97b9e50c1180 269 void flag2_activate(){flag2=true;}
MBroek 28:97b9e50c1180 270
MBroek 28:97b9e50c1180 271
MBroek 28:97b9e50c1180 272 // Dit doet de robot als hij niets doet ------------------------------------------------------
MBroek 28:97b9e50c1180 273 void robot_is_off(){
MBroek 28:97b9e50c1180 274 ledgreen.write(1);
MBroek 28:97b9e50c1180 275 ledred.write(0); // Indicator ik ben uit
MBroek 28:97b9e50c1180 276 motor1_speed_pin = 0; // Uitzetten vd motoren
MBroek 28:97b9e50c1180 277 motor2_speed_pin = 0;
MBroek 28:97b9e50c1180 278 }
MBroek 28:97b9e50c1180 279
MBroek 28:97b9e50c1180 280
MBroek 28:97b9e50c1180 281 // De initialiseren functie ------------------------------------------------------------
MBroek 28:97b9e50c1180 282 void initialise(){ // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
MBroek 28:97b9e50c1180 283 ref_pos_prev_m1 = initial_pos_m1;
MBroek 28:97b9e50c1180 284 ref_pos_prev_m2 = initial_pos_m2;
MBroek 28:97b9e50c1180 285 ledgreen.write(0);
MBroek 28:97b9e50c1180 286 encoder_motor1.reset();
MBroek 28:97b9e50c1180 287 encoder_motor2.reset();
MBroek 28:97b9e50c1180 288 motor1_speed_pin = 0;
MBroek 28:97b9e50c1180 289 }
MBroek 28:97b9e50c1180 290
MBroek 28:97b9e50c1180 291
MBroek 28:97b9e50c1180 292 // De functie die de motoren aanstuurt -------------------------------------------------
MBroek 28:97b9e50c1180 293 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 28:97b9e50c1180 294 if (flag1){
MBroek 28:97b9e50c1180 295 flag1 = false;
MBroek 28:97b9e50c1180 296 ledred.write(1);
MBroek 28:97b9e50c1180 297 switch (motor_that_runs){
MBroek 28:97b9e50c1180 298 case motor1 : // In deze case draait alleen motor 1
MBroek 28:97b9e50c1180 299 reference_pos_m1 = get_reference_position_m1(ref_pos_prev_m1, vrijheid_m1_rad);
MBroek 28:97b9e50c1180 300 position_motor1 = get_position_m1(rad_per_count);
MBroek 28:97b9e50c1180 301 PID_output_1 = PID_controller(reference_pos_m1, position_motor1, error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
MBroek 28:97b9e50c1180 302 //PID_output_2_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 er voor dat motor2 meedraait met motor1
MBroek 28:97b9e50c1180 303 set_motor1(PID_output_1);
MBroek 28:97b9e50c1180 304 set_motor2(PID_output_1);
MBroek 28:97b9e50c1180 305 encoder_motor2.reset();
MBroek 28:97b9e50c1180 306 break;
MBroek 28:97b9e50c1180 307 case motor2 :
MBroek 28:97b9e50c1180 308 reference_pos_m2 = get_reference_position_m2(ref_pos_prev_m2, vrijheid_m2_meter);
MBroek 28:97b9e50c1180 309 position_motor2 = get_position_m2(meter_per_count, position_motor2_prev);
MBroek 28:97b9e50c1180 310 PID_output_2 = PID_controller(reference_pos_m2, position_motor2, error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
MBroek 28:97b9e50c1180 311 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 28:97b9e50c1180 312 set_motor2(PID_output_2);
MBroek 28:97b9e50c1180 313 set_motor1(PID_output_1);
MBroek 28:97b9e50c1180 314 position_motor2_prev = position_motor2;
MBroek 28:97b9e50c1180 315 break;
MBroek 28:97b9e50c1180 316 }
MBroek 28:97b9e50c1180 317 }
MBroek 28:97b9e50c1180 318 }
MBroek 28:97b9e50c1180 319
MBroek 28:97b9e50c1180 320
MBroek 28:97b9e50c1180 321 // De HIDscope debug functie ----------------------------------------------------------------
MBroek 28:97b9e50c1180 322 void call_HIDscope(double input_1, double input_2, double input_3){ // Deze functie roept de HIDscope aan
MBroek 28:97b9e50c1180 323 if (flag2){
MBroek 28:97b9e50c1180 324 flag2 = false;
MBroek 28:97b9e50c1180 325 set_scope(input_1, input_2, input_3);
MBroek 28:97b9e50c1180 326 }
MBroek 28:97b9e50c1180 327 }
MBroek 28:97b9e50c1180 328
MBroek 28:97b9e50c1180 329
MBroek 28:97b9e50c1180 330
MBroek 28:97b9e50c1180 331
MBroek 28:97b9e50c1180 332 // DE MAIN =================================================================================================================
MBroek 28:97b9e50c1180 333 int main()
MBroek 28:97b9e50c1180 334 {
MBroek 28:97b9e50c1180 335 pc.baud(SERIALBAUD);
MBroek 28:97b9e50c1180 336
MBroek 28:97b9e50c1180 337 state_switch_button.fall(&state_changer); // Switcht states
MBroek 28:97b9e50c1180 338 motor_switch_button.fall(&motor_switch); // Switcht motors
MBroek 28:97b9e50c1180 339
MBroek 28:97b9e50c1180 340
MBroek 28:97b9e50c1180 341
MBroek 28:97b9e50c1180 342 motor_ticker.attach(&flag1_activate, T_motor_function); // Activeert de go-flag van motor positie
MBroek 28:97b9e50c1180 343 hidscope_ticker.attach(&flag2_activate,0.01); // Leest ref pos en mot pos uit, rekent PID uit en stuurt motoren aan.
MBroek 28:97b9e50c1180 344
MBroek 28:97b9e50c1180 345 while(1){
MBroek 28:97b9e50c1180 346 switch(states){
MBroek 28:97b9e50c1180 347 case off : // Dan staat het programma stil en gebeurt er niks
MBroek 28:97b9e50c1180 348 robot_is_off();
MBroek 28:97b9e50c1180 349 break;
MBroek 28:97b9e50c1180 350 case init : // Hier voert hij alleen het initialiseren van motor 1 uit
MBroek 28:97b9e50c1180 351 initialise();
MBroek 28:97b9e50c1180 352 break;
MBroek 28:97b9e50c1180 353 case run :
MBroek 28:97b9e50c1180 354 running_motors(); // Hier voert hij het programma voor het aansturen vd motors uit
MBroek 28:97b9e50c1180 355 break;
MBroek 28:97b9e50c1180 356
MBroek 28:97b9e50c1180 357 }
MBroek 28:97b9e50c1180 358 call_HIDscope(PID_output_1, reference_pos_m1, position_motor1);
MBroek 28:97b9e50c1180 359 }
MBroek 28:97b9e50c1180 360 }
MBroek 28:97b9e50c1180 361
MBroek 28:97b9e50c1180 362
MBroek 28:97b9e50c1180 363
MBroek 28:97b9e50c1180 364
MBroek 28:97b9e50c1180 365
MBroek 28:97b9e50c1180 366
MBroek 28:97b9e50c1180 367
MBroek 28:97b9e50c1180 368
MBroek 28:97b9e50c1180 369
MBroek 28:97b9e50c1180 370
MBroek 28:97b9e50c1180 371
MBroek 28:97b9e50c1180 372
MBroek 28:97b9e50c1180 373
MBroek 28:97b9e50c1180 374
MBroek 28:97b9e50c1180 375
MBroek 28:97b9e50c1180 376
MBroek 28:97b9e50c1180 377
MBroek 28:97b9e50c1180 378
MBroek 28:97b9e50c1180 379
MBroek 28:97b9e50c1180 380
MBroek 28:97b9e50c1180 381
MBroek 28:97b9e50c1180 382
MBroek 28:97b9e50c1180 383
MBroek 28:97b9e50c1180 384
MBroek 28:97b9e50c1180 385
MBroek 28:97b9e50c1180 386
MBroek 28:97b9e50c1180 387
MBroek 28:97b9e50c1180 388
MBroek 28:97b9e50c1180 389
MBroek 28:97b9e50c1180 390
MBroek 28:97b9e50c1180 391
MBroek 28:97b9e50c1180 392
MBroek 28:97b9e50c1180 393
MBroek 28:97b9e50c1180 394
MBroek 28:97b9e50c1180 395
MBroek 28:97b9e50c1180 396
MBroek 28:97b9e50c1180 397
MBroek 28:97b9e50c1180 398
MBroek 28:97b9e50c1180 399
MBroek 28:97b9e50c1180 400
MBroek 28:97b9e50c1180 401
MBroek 28:97b9e50c1180 402
MBroek 28:97b9e50c1180 403
MBroek 28:97b9e50c1180 404
MBroek 28:97b9e50c1180 405
MBroek 28:97b9e50c1180 406
MBroek 28:97b9e50c1180 407
MBroek 28:97b9e50c1180 408
MBroek 28:97b9e50c1180 409
MBroek 28:97b9e50c1180 410
MBroek 28:97b9e50c1180 411
MBroek 28:97b9e50c1180 412
MBroek 28:97b9e50c1180 413
MBroek 28:97b9e50c1180 414
MBroek 28:97b9e50c1180 415
MBroek 28:97b9e50c1180 416
MBroek 28:97b9e50c1180 417
MBroek 28:97b9e50c1180 418
MBroek 28:97b9e50c1180 419
MBroek 28:97b9e50c1180 420
MBroek 28:97b9e50c1180 421
MBroek 28:97b9e50c1180 422