Arnoud Domhof / Mbed 2 deprecated project_demomode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM MODSERIAL QEI mbed

main_demo.cpp@5:cd329205f037, 2018-11-01 (annotated)

Committer:
arnouddomhof
Date:
Thu Nov 01 11:03:23 2018 +0000
Revision:
5:cd329205f037
Parent:
4:98b3dfdd9ae1
Child:
6:5431ddb9d881
Hij heen en weer en gaat alle 4 de punten af. Hij gaat alleen nog niet naar boven en beneden.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AppelSab 0:755bc7c0f555 1 #include "mbed.h"
AppelSab 0:755bc7c0f555 2 #include "FastPWM.h"
AppelSab 0:755bc7c0f555 3 #include "MODSERIAL.h"
AppelSab 0:755bc7c0f555 4 #include "QEI.h"
AppelSab 0:755bc7c0f555 5 #include "math.h"
arnouddomhof 5:cd329205f037 6 //toch forken
AppelSab 0:755bc7c0f555 7 // --------------------------------------------------
AppelSab 0:755bc7c0f555 8 // ----------------- SET UP -------------------------
AppelSab 0:755bc7c0f555 9 QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, counts/rev)
AppelSab 0:755bc7c0f555 10 QEI Encoder2(D9, D8, NC, 4200) ; // Encoder motor 2, (pin 2A, pin 2B, counts/rev)
AppelSab 0:755bc7c0f555 11 DigitalOut directionM1(D4);
AppelSab 0:755bc7c0f555 12 DigitalOut directionM2(D7);
AppelSab 0:755bc7c0f555 13 FastPWM motor1_pwm(D5);
AppelSab 0:755bc7c0f555 14 FastPWM motor2_pwm(D6);
arnouddomhof 5:cd329205f037 15 MODSERIAL pc(USBTX, USBRX);
AppelSab 2:7e86ced5841f 16 DigitalOut ledr(LED_RED);
AppelSab 2:7e86ced5841f 17 DigitalOut ledg(LED_GREEN);
AppelSab 2:7e86ced5841f 18 DigitalOut ledb(LED_BLUE);
AppelSab 0:755bc7c0f555 19
AppelSab 0:755bc7c0f555 20 // Tickers
AppelSab 0:755bc7c0f555 21 Ticker Demo;
AppelSab 0:755bc7c0f555 22 Ticker Write;
AppelSab 0:755bc7c0f555 23
AppelSab 0:755bc7c0f555 24 // ---------------------------------------------------
AppelSab 0:755bc7c0f555 25 // ----------------- GLOBAL VARIABLES ----------------
AppelSab 0:755bc7c0f555 26 volatile int counts1;
AppelSab 0:755bc7c0f555 27 volatile int counts2;
AppelSab 0:755bc7c0f555 28 volatile double theta1;
AppelSab 0:755bc7c0f555 29 volatile double theta2;
AppelSab 0:755bc7c0f555 30 const double pi = 3.14159265359;
AppelSab 0:755bc7c0f555 31 volatile double error1;
AppelSab 0:755bc7c0f555 32 volatile double error2;
AppelSab 0:755bc7c0f555 33 double point1x = 200.0;
AppelSab 0:755bc7c0f555 34 double point1y = 200.0;
arnouddomhof 5:cd329205f037 35 double point2x = 350.0;
arnouddomhof 5:cd329205f037 36 double point2y = 200.0;
arnouddomhof 5:cd329205f037 37 double point3x = 350.0;
arnouddomhof 5:cd329205f037 38 double point3y = 100.0;
arnouddomhof 5:cd329205f037 39 double point4x = 200.0;
arnouddomhof 5:cd329205f037 40 double point4y = 100.0;
arnouddomhof 3:fd1380ffb200 41 volatile int track = 1;
AppelSab 0:755bc7c0f555 42 const double x0 = 80.0; //zero x position after homing
AppelSab 0:755bc7c0f555 43 const double y0 = 141.0; //zero y position after homing
AppelSab 0:755bc7c0f555 44 volatile double setpointx = x0;
AppelSab 0:755bc7c0f555 45 volatile double setpointy = y0;
AppelSab 0:755bc7c0f555 46 volatile double U1;
AppelSab 0:755bc7c0f555 47 volatile double U2;
AppelSab 0:755bc7c0f555 48
AppelSab 0:755bc7c0f555 49 // Inverse Kinematics
AppelSab 0:755bc7c0f555 50 volatile double q1_diff;
AppelSab 0:755bc7c0f555 51 volatile double q2_diff;
AppelSab 0:755bc7c0f555 52 double sq = 2.0; //to square numbers
AppelSab 0:755bc7c0f555 53 const double L1 = 250.0; //length of the first link
AppelSab 0:755bc7c0f555 54 const double L3 = 350.0; //length of the second link
AppelSab 0:755bc7c0f555 55
AppelSab 0:755bc7c0f555 56 // Reference angles of the starting position
AppelSab 0:755bc7c0f555 57 double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
AppelSab 0:755bc7c0f555 58 double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
AppelSab 0:755bc7c0f555 59 double q2_0_enc = q2_0 + q1_0;
AppelSab 0:755bc7c0f555 60
AppelSab 0:755bc7c0f555 61 // --------------------------------------------------------------------
AppelSab 0:755bc7c0f555 62 // ---------------Read out encoders------------------------------------
AppelSab 0:755bc7c0f555 63 // --------------------------------------------------------------------
AppelSab 0:755bc7c0f555 64 double counts2angle1()
AppelSab 0:755bc7c0f555 65 {
AppelSab 0:755bc7c0f555 66 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
AppelSab 0:755bc7c0f555 67 theta1 = -(double(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
AppelSab 0:755bc7c0f555 68 return theta1;
AppelSab 0:755bc7c0f555 69 }
AppelSab 0:755bc7c0f555 70
AppelSab 0:755bc7c0f555 71 double counts2angle2()
AppelSab 0:755bc7c0f555 72 {
AppelSab 0:755bc7c0f555 73 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
AppelSab 0:755bc7c0f555 74 theta2 = (double(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
AppelSab 0:755bc7c0f555 75 return theta2;
AppelSab 0:755bc7c0f555 76 }
AppelSab 0:755bc7c0f555 77
AppelSab 0:755bc7c0f555 78 // -------------------------------------------------------------------------
AppelSab 0:755bc7c0f555 79 // -------------- Determine Setpoints --------------------------------------
AppelSab 0:755bc7c0f555 80 // -------------------------------------------------------------------------
arnouddomhof 5:cd329205f037 81 double determinedemosetx(double setpointx, double setpointy)
AppelSab 0:755bc7c0f555 82 {
arnouddomhof 5:cd329205f037 83
arnouddomhof 5:cd329205f037 84 if (setpointx < point1x && track == 1){
arnouddomhof 5:cd329205f037 85 setpointx = setpointx + 0.1;
AppelSab 1:7afdfab34bf7 86 }
arnouddomhof 3:fd1380ffb200 87
arnouddomhof 5:cd329205f037 88 // Van punt 1 naar punt 2.
arnouddomhof 5:cd329205f037 89 if (setpointy >= point1y - 0.3 && setpointx >= point1x - 0.3 && setpointy <= point1y + 0.3 && setpointx <= point1x + 0.3 && track == 1){
arnouddomhof 5:cd329205f037 90 setpointx = setpointx + 0.1;
arnouddomhof 5:cd329205f037 91 track = 12;
arnouddomhof 5:cd329205f037 92 }
arnouddomhof 5:cd329205f037 93 if (setpointx < point2x && track == 12){
arnouddomhof 5:cd329205f037 94 setpointx = setpointx + 0.2;
arnouddomhof 3:fd1380ffb200 95 }
arnouddomhof 3:fd1380ffb200 96
arnouddomhof 5:cd329205f037 97 // Van punt 2 naar punt 1.
arnouddomhof 5:cd329205f037 98 if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3 && track == 12)
AppelSab 1:7afdfab34bf7 99 {
arnouddomhof 5:cd329205f037 100 setpointx = point3x;
AppelSab 1:7afdfab34bf7 101 track = 23;
AppelSab 1:7afdfab34bf7 102 }
AppelSab 1:7afdfab34bf7 103 if (setpointy > point3y && track == 23)
AppelSab 1:7afdfab34bf7 104 {
arnouddomhof 5:cd329205f037 105 setpointx = point3x; // Van punt 1 naar punt 2 op dezelfde y blijven.
AppelSab 1:7afdfab34bf7 106 }
arnouddomhof 5:cd329205f037 107
arnouddomhof 5:cd329205f037 108
AppelSab 0:755bc7c0f555 109 // Van punt 3 naar punt 4.
AppelSab 0:755bc7c0f555 110 if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
AppelSab 0:755bc7c0f555 111 {
AppelSab 0:755bc7c0f555 112 setpointx = setpointx - 0.1; // Van punt 1 naar punt 2 op dezelfde y blijven.
arnouddomhof 5:cd329205f037 113 track = 34;
arnouddomhof 5:cd329205f037 114 }
arnouddomhof 5:cd329205f037 115
arnouddomhof 5:cd329205f037 116 if (setpointy > point3y && track == 34)
arnouddomhof 5:cd329205f037 117 {
arnouddomhof 5:cd329205f037 118 setpointx = setpointx - 0.1;
arnouddomhof 5:cd329205f037 119 }
arnouddomhof 5:cd329205f037 120
arnouddomhof 5:cd329205f037 121 if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3 && track == 34)
arnouddomhof 5:cd329205f037 122 {
arnouddomhof 5:cd329205f037 123 track = 1;
arnouddomhof 5:cd329205f037 124 }
arnouddomhof 5:cd329205f037 125
arnouddomhof 5:cd329205f037 126 return setpointx;
arnouddomhof 5:cd329205f037 127 }
arnouddomhof 5:cd329205f037 128
arnouddomhof 5:cd329205f037 129 double determinedemosety(double setpointx, double setpointy)
arnouddomhof 5:cd329205f037 130 {
arnouddomhof 5:cd329205f037 131 // Van reference positie naar punt 1.
arnouddomhof 5:cd329205f037 132 if(setpointy < point1y && track == 1){
arnouddomhof 5:cd329205f037 133 setpointy = setpointy + (0.2);
arnouddomhof 5:cd329205f037 134 }
arnouddomhof 5:cd329205f037 135
arnouddomhof 5:cd329205f037 136 // Van punt 1 naar punt 2.
arnouddomhof 5:cd329205f037 137 if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3 && track == 1){
arnouddomhof 5:cd329205f037 138 ledg = 1;
arnouddomhof 5:cd329205f037 139 ledr = 0;
arnouddomhof 5:cd329205f037 140 setpointy = point2y; // Van punt 1 naar punt 2 op dezelfde y blijven.
arnouddomhof 5:cd329205f037 141 track = 12;
arnouddomhof 5:cd329205f037 142 }
arnouddomhof 5:cd329205f037 143 if (setpointx < point2x && track == 12){
arnouddomhof 5:cd329205f037 144 setpointy = point2y;
arnouddomhof 5:cd329205f037 145 }
arnouddomhof 5:cd329205f037 146
arnouddomhof 5:cd329205f037 147 // Van punt 2 naar punt 3.
arnouddomhof 5:cd329205f037 148 if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3){
arnouddomhof 5:cd329205f037 149 ledr = 1;
arnouddomhof 5:cd329205f037 150 ledg = 0;
arnouddomhof 5:cd329205f037 151 setpointx = point3x;
arnouddomhof 5:cd329205f037 152 track = 23;
arnouddomhof 5:cd329205f037 153 }
arnouddomhof 5:cd329205f037 154 if ((setpointy > point3y) && (track == 23))
arnouddomhof 5:cd329205f037 155 {
arnouddomhof 5:cd329205f037 156 setpointy = setpointy + (-0.2);
arnouddomhof 5:cd329205f037 157 track = 23;
arnouddomhof 5:cd329205f037 158 }
arnouddomhof 5:cd329205f037 159
arnouddomhof 5:cd329205f037 160 // Van punt 3 naar punt 4.
arnouddomhof 5:cd329205f037 161 if ((fabs(setpointx - point3x) <= 0.3) && (fabs(setpointy - point3y) <= 0.3) && (track == 23))
arnouddomhof 5:cd329205f037 162 {
arnouddomhof 5:cd329205f037 163 ledg = 1;
arnouddomhof 5:cd329205f037 164 ledr = 1;
arnouddomhof 5:cd329205f037 165 ledb = 0;
AppelSab 2:7e86ced5841f 166 setpointy = setpointy;
AppelSab 0:755bc7c0f555 167 track = 34;
AppelSab 0:755bc7c0f555 168 }
AppelSab 0:755bc7c0f555 169 if (setpointy > point3y && track == 34)
AppelSab 0:755bc7c0f555 170 {
AppelSab 2:7e86ced5841f 171 setpointy = setpointy;
arnouddomhof 5:cd329205f037 172 }
arnouddomhof 5:cd329205f037 173
arnouddomhof 5:cd329205f037 174
arnouddomhof 5:cd329205f037 175 if ((fabs(setpointx - point4x) <= 0.3) && (fabs(setpointy - point4y) <= 0.3) && (track == 34))
arnouddomhof 5:cd329205f037 176 {
arnouddomhof 5:cd329205f037 177 ledg = 1;
arnouddomhof 5:cd329205f037 178 ledr = 1;
arnouddomhof 5:cd329205f037 179 ledb = 1;
arnouddomhof 5:cd329205f037 180 track = 1;
AppelSab 0:755bc7c0f555 181 }
arnouddomhof 3:fd1380ffb200 182
arnouddomhof 5:cd329205f037 183 return setpointy;
arnouddomhof 5:cd329205f037 184
arnouddomhof 5:cd329205f037 185 }
AppelSab 0:755bc7c0f555 186
AppelSab 0:755bc7c0f555 187 // -----------------------------------------------------------------
AppelSab 0:755bc7c0f555 188 // --------------------------- PI controllers ----------------------
AppelSab 0:755bc7c0f555 189 // -----------------------------------------------------------------
AppelSab 0:755bc7c0f555 190 double PI_controller1(double error1)
AppelSab 0:755bc7c0f555 191 {
AppelSab 0:755bc7c0f555 192 static double error_integral1 = 0;
AppelSab 0:755bc7c0f555 193
AppelSab 0:755bc7c0f555 194 // Proportional part
AppelSab 0:755bc7c0f555 195 double Kp1 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 196 double u_p1 = Kp1*error1; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
AppelSab 0:755bc7c0f555 197
AppelSab 0:755bc7c0f555 198 // Integral part
AppelSab 0:755bc7c0f555 199 double Ki1 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 200 double Ts1 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
AppelSab 0:755bc7c0f555 201 error_integral1 = error_integral1 + error1 * Ts1;
AppelSab 0:755bc7c0f555 202 double u_i1 = Ki1 * error_integral1;
AppelSab 0:755bc7c0f555 203
AppelSab 0:755bc7c0f555 204 // Sum
AppelSab 0:755bc7c0f555 205 U1 = u_p1 + u_i1;
AppelSab 0:755bc7c0f555 206
AppelSab 0:755bc7c0f555 207 // Return
AppelSab 0:755bc7c0f555 208 return U1;
AppelSab 0:755bc7c0f555 209 }
AppelSab 0:755bc7c0f555 210 double PI_controller2(double error2)
AppelSab 0:755bc7c0f555 211 {
AppelSab 0:755bc7c0f555 212 static double error_integral2 = 0;
AppelSab 0:755bc7c0f555 213
AppelSab 0:755bc7c0f555 214 // Proportional part
AppelSab 0:755bc7c0f555 215 double Kp2 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 216 double u_p2 = Kp2*error2; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
AppelSab 0:755bc7c0f555 217
AppelSab 0:755bc7c0f555 218 // Integral part
AppelSab 0:755bc7c0f555 219 double Ki2 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 220 double Ts2 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
AppelSab 0:755bc7c0f555 221 error_integral2 = error_integral2 + error2 * Ts2;
AppelSab 0:755bc7c0f555 222 double u_i2 = Ki2 * error_integral2;
AppelSab 0:755bc7c0f555 223
AppelSab 0:755bc7c0f555 224 // Sum +
AppelSab 0:755bc7c0f555 225 U2 = u_p2 + u_i2;
AppelSab 0:755bc7c0f555 226
AppelSab 0:755bc7c0f555 227 // Return
AppelSab 0:755bc7c0f555 228 return U2;
AppelSab 0:755bc7c0f555 229 }
AppelSab 0:755bc7c0f555 230 // ------------------------------------------------------------
AppelSab 0:755bc7c0f555 231 // ------------ Inverse Kinematics ----------------------------
AppelSab 0:755bc7c0f555 232 // ------------------------------------------------------------
AppelSab 0:755bc7c0f555 233 double makeAngleq1(double x, double y){
AppelSab 0:755bc7c0f555 234 double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
AppelSab 0:755bc7c0f555 235 q1_diff = -2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
AppelSab 0:755bc7c0f555 236 return q1_diff;
AppelSab 0:755bc7c0f555 237 }
AppelSab 0:755bc7c0f555 238
AppelSab 0:755bc7c0f555 239 double makeAngleq2(double x, double y){
AppelSab 0:755bc7c0f555 240 double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
AppelSab 0:755bc7c0f555 241 double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
AppelSab 0:755bc7c0f555 242 double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
AppelSab 0:755bc7c0f555 243 q2_diff = (2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint
AppelSab 0:755bc7c0f555 244 return -q2_diff;
AppelSab 0:755bc7c0f555 245 }
AppelSab 0:755bc7c0f555 246
AppelSab 0:755bc7c0f555 247
AppelSab 0:755bc7c0f555 248 // -----------------------------------------------
AppelSab 0:755bc7c0f555 249 // ------------ RUN MOTORS -----------------------
AppelSab 0:755bc7c0f555 250 // -----------------------------------------------
AppelSab 0:755bc7c0f555 251 void motoraansturing()
AppelSab 0:755bc7c0f555 252 {
arnouddomhof 5:cd329205f037 253 setpointx = determinedemosetx(setpointx, setpointy);
arnouddomhof 5:cd329205f037 254 setpointy = determinedemosety(setpointx, setpointy);
AppelSab 0:755bc7c0f555 255 q1_diff = makeAngleq1(setpointx, setpointy);
AppelSab 0:755bc7c0f555 256 q2_diff = makeAngleq2(setpointx, setpointy);
AppelSab 0:755bc7c0f555 257
AppelSab 0:755bc7c0f555 258 theta2 = counts2angle2();
AppelSab 0:755bc7c0f555 259 error2 = q2_diff - theta2;
AppelSab 0:755bc7c0f555 260 theta1 = counts2angle1();
AppelSab 0:755bc7c0f555 261 error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as.
AppelSab 0:755bc7c0f555 262
AppelSab 0:755bc7c0f555 263 U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
AppelSab 0:755bc7c0f555 264 U2 = PI_controller2(error2);
AppelSab 0:755bc7c0f555 265
AppelSab 0:755bc7c0f555 266 motor1_pwm.write(fabs(U1)); // Motor aansturen
AppelSab 0:755bc7c0f555 267 directionM1 = U1 > 0.0f; // Richting van de motor bepalen
AppelSab 0:755bc7c0f555 268 motor2_pwm.write(fabs(U2));
arnouddomhof 5:cd329205f037 269 directionM2 = U2 > 0.0f;
AppelSab 0:755bc7c0f555 270 }
AppelSab 0:755bc7c0f555 271
AppelSab 0:755bc7c0f555 272
AppelSab 0:755bc7c0f555 273 void rundemo()
AppelSab 0:755bc7c0f555 274 {
AppelSab 0:755bc7c0f555 275 motoraansturing();
AppelSab 0:755bc7c0f555 276 }
AppelSab 0:755bc7c0f555 277
AppelSab 0:755bc7c0f555 278
AppelSab 0:755bc7c0f555 279 int main()
AppelSab 0:755bc7c0f555 280 {
AppelSab 0:755bc7c0f555 281 pc.baud(115200);
AppelSab 0:755bc7c0f555 282 motor1_pwm.period_us(60); // Period is 60 microseconde
AppelSab 0:755bc7c0f555 283 motor2_pwm.period_us(60);
AppelSab 0:755bc7c0f555 284 Demo.attach(&rundemo, 0.005f);
AppelSab 0:755bc7c0f555 285
arnouddomhof 5:cd329205f037 286 ledr = 1;
arnouddomhof 5:cd329205f037 287 ledg = 1;
arnouddomhof 5:cd329205f037 288 ledb = 1;
arnouddomhof 3:fd1380ffb200 289
AppelSab 0:755bc7c0f555 290 while (true) {
arnouddomhof 5:cd329205f037 291 pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
arnouddomhof 3:fd1380ffb200 292 if (track == 1) {
arnouddomhof 3:fd1380ffb200 293 pc.printf("Gaat naar positie 1\r\n");
arnouddomhof 3:fd1380ffb200 294 }
arnouddomhof 3:fd1380ffb200 295 else if (track == 12) {
arnouddomhof 3:fd1380ffb200 296 pc.printf("Gaat naar positie 2\r\n");
arnouddomhof 3:fd1380ffb200 297 }
arnouddomhof 5:cd329205f037 298 else if (track == 23) {
arnouddomhof 5:cd329205f037 299 pc.printf("Gaat naar positie 3\r\n");
arnouddomhof 5:cd329205f037 300 }
arnouddomhof 5:cd329205f037 301 else if (track == 34) {
arnouddomhof 5:cd329205f037 302 pc.printf("Gaat naar positie 4\r\n");
arnouddomhof 5:cd329205f037 303 }
arnouddomhof 3:fd1380ffb200 304 wait(0.5f);
AppelSab 0:755bc7c0f555 305 }
AppelSab 0:755bc7c0f555 306 }