Ramon Waninge / Mbed 2 deprecated Milestone1

Motor aansturen met mbed

Dependencies:   FastPWM mbed QEI biquadFilter HIDScope MODSERIAL

main.cpp@50:522bb6eebda6, 2018-11-01 (annotated)

Committer:
efvanmarrewijk
Date:
Thu Nov 01 08:46:58 2018 +0000
Revision:
50:522bb6eebda6
Parent:
49:48363ca21a15 
Latest version just before merge, without calibration code and with code for movement of 3 motors, 3rd motor is TURNED OFF (after calibration it can be turned on again)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
efvanmarrewijk 16:720365110953 1 // Inclusion of libraries
efvanmarrewijk 31:91ad5b188bd9 2 #include "mbed.h"
efvanmarrewijk 31:91ad5b188bd9 3 #include "FastPWM.h"
efvanmarrewijk 31:91ad5b188bd9 4 #include "QEI.h" // Includes library for encoder
efvanmarrewijk 31:91ad5b188bd9 5 #include "MODSERIAL.h"
efvanmarrewijk 31:91ad5b188bd9 6 #include "HIDScope.h"
efvanmarrewijk 31:91ad5b188bd9 7 #include "BiQuad.h"
Ramonwaninge 0:3ea1bbfbeaae 8
efvanmarrewijk 14:e21cb701ccb8 9 // Input
efvanmarrewijk 27:3430dfb4c9fb 10 AnalogIn pot1(A1);
efvanmarrewijk 27:3430dfb4c9fb 11 AnalogIn pot2(A2);
efvanmarrewijk 21:363271dcfe1f 12 InterruptIn button1(D0);
efvanmarrewijk 21:363271dcfe1f 13 InterruptIn button2(D1);
efvanmarrewijk 31:91ad5b188bd9 14 InterruptIn emergencybutton(SW2); //The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible
efvanmarrewijk 21:363271dcfe1f 15
efvanmarrewijk 32:a5b411833d1e 16 DigitalIn pin8(D8); // Encoder 1 B
efvanmarrewijk 32:a5b411833d1e 17 DigitalIn pin9(D9); // Encoder 1 A
efvanmarrewijk 32:a5b411833d1e 18 DigitalIn pin10(D10); // Encoder 2 B
efvanmarrewijk 32:a5b411833d1e 19 DigitalIn pin11(D11); // Encoder 2 A
efvanmarrewijk 32:a5b411833d1e 20 DigitalIn pin12(D12); // Encoder 3 B
efvanmarrewijk 32:a5b411833d1e 21 DigitalIn pin13(D13); // Encoder 3 A
efvanmarrewijk 9:65c52c1f4a57 22
efvanmarrewijk 14:e21cb701ccb8 23 // Output
efvanmarrewijk 35:ba556f2d0fcc 24 DigitalOut pin2(D2); // Motor 3 direction = motor flip
efvanmarrewijk 43:e8f2193822b7 25 FastPWM pin3(A5); // Motor 3 pwm = motor flip
efvanmarrewijk 35:ba556f2d0fcc 26 DigitalOut pin4(D4); // Motor 2 direction = motor right
efvanmarrewijk 35:ba556f2d0fcc 27 FastPWM pin5(D5); // Motor 2 pwm = motor right
efvanmarrewijk 35:ba556f2d0fcc 28 FastPWM pin6(D6); // Motor 1 pwm = motor left
efvanmarrewijk 35:ba556f2d0fcc 29 DigitalOut pin7(D7); // Motor 1 direction = motor left
efvanmarrewijk 26:b48708ed51ff 30
efvanmarrewijk 26:b48708ed51ff 31 DigitalOut greenled(LED_GREEN,1);
efvanmarrewijk 26:b48708ed51ff 32 DigitalOut redled(LED_RED,1);
efvanmarrewijk 26:b48708ed51ff 33 DigitalOut blueled(LED_BLUE,1);
Ramonwaninge 2:d8a552d1d33a 34
efvanmarrewijk 16:720365110953 35 // Utilisation of libraries
efvanmarrewijk 16:720365110953 36 MODSERIAL pc(USBTX, USBRX);
efvanmarrewijk 35:ba556f2d0fcc 37 QEI Encoderl(D9,D8,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 39:dcf3e5019a63 38 QEI Encoderr(D10,D11,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 43:e8f2193822b7 39 QEI Encoderf(D12,D13,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 43:e8f2193822b7 40 Ticker motorl;
efvanmarrewijk 43:e8f2193822b7 41 Ticker motorr;
efvanmarrewijk 43:e8f2193822b7 42 Ticker motorf;
efvanmarrewijk 24:d255752065d1 43 Ticker encoders;
efvanmarrewijk 9:65c52c1f4a57 44
efvanmarrewijk 16:720365110953 45 // Global variables
efvanmarrewijk 50:522bb6eebda6 46 const float PI = 3.14159265358979f;
efvanmarrewijk 32:a5b411833d1e 47 float u3 = 0.0f; // Normalised variable for the movement of motor 3
efvanmarrewijk 32:a5b411833d1e 48 const float fCountsRad = 4200.0f;
efvanmarrewijk 49:48363ca21a15 49 const float dt = 0.002f;
efvanmarrewijk 16:720365110953 50
efvanmarrewijk 43:e8f2193822b7 51 float currentanglel;
efvanmarrewijk 43:e8f2193822b7 52 float errorl;
efvanmarrewijk 43:e8f2193822b7 53 float currentangler;
efvanmarrewijk 43:e8f2193822b7 54 float errorr;
efvanmarrewijk 43:e8f2193822b7 55 float currentanglef;
efvanmarrewijk 43:e8f2193822b7 56 float errorf;
efvanmarrewijk 49:48363ca21a15 57 float Kp;
efvanmarrewijk 49:48363ca21a15 58 float Kd;
efvanmarrewijk 43:e8f2193822b7 59
efvanmarrewijk 16:720365110953 60 // Functions
efvanmarrewijk 32:a5b411833d1e 61 void Emergency() // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
efvanmarrewijk 32:a5b411833d1e 62 { greenled = 1; // Red light on
efvanmarrewijk 30:c4a3e868ef04 63 blueled = 1;
efvanmarrewijk 30:c4a3e868ef04 64 redled = 0;
efvanmarrewijk 32:a5b411833d1e 65 pin3 = 0; // All motor forces to zero
efvanmarrewijk 30:c4a3e868ef04 66 pin5 = 0;
efvanmarrewijk 30:c4a3e868ef04 67 pin6 = 0;
efvanmarrewijk 32:a5b411833d1e 68 exit (0); // Abort mission!!
efvanmarrewijk 30:c4a3e868ef04 69 }
efvanmarrewijk 30:c4a3e868ef04 70
efvanmarrewijk 31:91ad5b188bd9 71 // Subfunctions
efvanmarrewijk 35:ba556f2d0fcc 72 int Countslinput() // Gets the counts from encoder 1
efvanmarrewijk 35:ba556f2d0fcc 73 { int countsl;
efvanmarrewijk 35:ba556f2d0fcc 74 countsl = Encoderl.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 75 return countsl;
efvanmarrewijk 27:3430dfb4c9fb 76 }
efvanmarrewijk 35:ba556f2d0fcc 77 int Countsrinput() // Gets the counts from encoder 2
efvanmarrewijk 35:ba556f2d0fcc 78 { int countsr;
efvanmarrewijk 35:ba556f2d0fcc 79 countsr = Encoderr.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 80 return countsr;
efvanmarrewijk 27:3430dfb4c9fb 81 }
efvanmarrewijk 35:ba556f2d0fcc 82 int Countsfinput() // Gets the counts from encoder 3
efvanmarrewijk 35:ba556f2d0fcc 83 { int countsf;
efvanmarrewijk 35:ba556f2d0fcc 84 countsf = Encoderf.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 85 return countsf;
efvanmarrewijk 27:3430dfb4c9fb 86 }
efvanmarrewijk 27:3430dfb4c9fb 87
efvanmarrewijk 50:522bb6eebda6 88 float CurrentAngle(float counts) // Calculates the current angle of the motor (between -2*PI to 2*PI) based on the counts from the encoder
efvanmarrewijk 50:522bb6eebda6 89 { float angle = ((float)counts*2.0f*PI)/fCountsRad;
efvanmarrewijk 50:522bb6eebda6 90 while (angle > 2.0f*PI)
efvanmarrewijk 50:522bb6eebda6 91 { angle = angle-2.0f*PI;
efvanmarrewijk 27:3430dfb4c9fb 92 }
efvanmarrewijk 50:522bb6eebda6 93 while (angle < -2.0f*PI)
efvanmarrewijk 50:522bb6eebda6 94 { angle = angle+2.0f*PI;
efvanmarrewijk 30:c4a3e868ef04 95 }
efvanmarrewijk 30:c4a3e868ef04 96 return angle;
efvanmarrewijk 27:3430dfb4c9fb 97 }
efvanmarrewijk 27:3430dfb4c9fb 98
efvanmarrewijk 32:a5b411833d1e 99 float ErrorCalc(float refvalue,float CurAngle) // Calculates the error of the system, based on the current angle and the reference value
efvanmarrewijk 32:a5b411833d1e 100 { float error = refvalue - CurAngle;
efvanmarrewijk 30:c4a3e868ef04 101 return error;
efvanmarrewijk 30:c4a3e868ef04 102 }
efvanmarrewijk 30:c4a3e868ef04 103
efvanmarrewijk 32:a5b411833d1e 104 float Kpcontr() // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
efvanmarrewijk 49:48363ca21a15 105 { float Kp = 40.0f*pot2;
efvanmarrewijk 30:c4a3e868ef04 106 return Kp;
efvanmarrewijk 30:c4a3e868ef04 107 }
efvanmarrewijk 30:c4a3e868ef04 108
efvanmarrewijk 32:a5b411833d1e 109 float Kdcontr() // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 49:48363ca21a15 110 { float Kd = 10.0f*pot1;
efvanmarrewijk 30:c4a3e868ef04 111 return Kd;
efvanmarrewijk 30:c4a3e868ef04 112 }
efvanmarrewijk 43:e8f2193822b7 113
efvanmarrewijk 43:e8f2193822b7 114 float PIDcontrollerl(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 50:522bb6eebda6 115 { //Kp = Kpcontr();
efvanmarrewijk 50:522bb6eebda6 116 float Kp = 19.24f;
efvanmarrewijk 49:48363ca21a15 117 float Ki = 1.02f;
efvanmarrewijk 50:522bb6eebda6 118 float Kd = 0.827f;
efvanmarrewijk 50:522bb6eebda6 119 //Kd = Kdcontr();
efvanmarrewijk 43:e8f2193822b7 120 float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 43:e8f2193822b7 121 static float error_integrall = 0.0;
efvanmarrewijk 43:e8f2193822b7 122 static float error_prevl = error; // initialization with this value only done once!
efvanmarrewijk 43:e8f2193822b7 123 static BiQuad PIDfilterl(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 43:e8f2193822b7 124 // Proportional part:
efvanmarrewijk 43:e8f2193822b7 125 float u_k = Kp * error;
efvanmarrewijk 43:e8f2193822b7 126 // Integral part
efvanmarrewijk 43:e8f2193822b7 127 error_integrall = error_integrall + error * dt;
efvanmarrewijk 43:e8f2193822b7 128 float u_i = Ki * error_integrall;
efvanmarrewijk 43:e8f2193822b7 129 // Derivative part
efvanmarrewijk 43:e8f2193822b7 130 float error_derivative = (error - error_prevl)/dt;
efvanmarrewijk 43:e8f2193822b7 131 float filtered_error_derivative = PIDfilterl.step(error_derivative);
efvanmarrewijk 43:e8f2193822b7 132 float u_d = Kd * filtered_error_derivative;
efvanmarrewijk 43:e8f2193822b7 133 error_prevl = error;
efvanmarrewijk 43:e8f2193822b7 134 // Sum all parts and return it
efvanmarrewijk 43:e8f2193822b7 135 return u_k + u_i + u_d;
efvanmarrewijk 43:e8f2193822b7 136 }
efvanmarrewijk 43:e8f2193822b7 137
efvanmarrewijk 43:e8f2193822b7 138 float DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 50:522bb6eebda6 139 { float angle = (pot1*2.0f*PI)-PI;
efvanmarrewijk 43:e8f2193822b7 140 return angle;
efvanmarrewijk 43:e8f2193822b7 141 }
efvanmarrewijk 43:e8f2193822b7 142
efvanmarrewijk 43:e8f2193822b7 143 void turnl() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 43:e8f2193822b7 144 {
efvanmarrewijk 50:522bb6eebda6 145 //float refvaluel = 0.5f*PI;
efvanmarrewijk 50:522bb6eebda6 146 float refvaluel = -DesiredAnglel(); // different minus sign per motor
efvanmarrewijk 50:522bb6eebda6 147 int countsl = Countslinput(); // different encoder pins per motor
efvanmarrewijk 50:522bb6eebda6 148 currentanglel = CurrentAngle(countsl); // different minus sign per motor
efvanmarrewijk 50:522bb6eebda6 149 float PIDcontroll = PIDcontrollerl(refvaluel,currentanglel); // same for every motor
efvanmarrewijk 50:522bb6eebda6 150 errorl = ErrorCalc(refvaluel,currentanglel); // same for every motor
efvanmarrewijk 43:e8f2193822b7 151
efvanmarrewijk 50:522bb6eebda6 152 pin6 = fabs(PIDcontroll); // different pins for every motor
efvanmarrewijk 50:522bb6eebda6 153 pin7 = PIDcontroll > 0.0f; // different pins for every motor
efvanmarrewijk 43:e8f2193822b7 154 }
efvanmarrewijk 43:e8f2193822b7 155
efvanmarrewijk 43:e8f2193822b7 156 float PIDcontrollerr(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 50:522bb6eebda6 157 { //Kp = Kpcontr();
efvanmarrewijk 50:522bb6eebda6 158 float Kp = 19.24f;
efvanmarrewijk 50:522bb6eebda6 159 float Ki = 1.02f;
efvanmarrewijk 50:522bb6eebda6 160 float Kd = 0.827f;
efvanmarrewijk 50:522bb6eebda6 161 //Kd = Kdcontr();
efvanmarrewijk 32:a5b411833d1e 162 float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 43:e8f2193822b7 163 static float error_integralr = 0.0;
efvanmarrewijk 43:e8f2193822b7 164 static float error_prevr = error; // initialization with this value only done once!
efvanmarrewijk 43:e8f2193822b7 165 static BiQuad PIDfilterr(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 27:3430dfb4c9fb 166 // Proportional part:
efvanmarrewijk 30:c4a3e868ef04 167 float u_k = Kp * error;
efvanmarrewijk 27:3430dfb4c9fb 168 // Integral part
efvanmarrewijk 43:e8f2193822b7 169 error_integralr = error_integralr + error * dt;
efvanmarrewijk 43:e8f2193822b7 170 float u_i = Ki * error_integralr;
efvanmarrewijk 30:c4a3e868ef04 171 // Derivative part
efvanmarrewijk 43:e8f2193822b7 172 float error_derivative = (error - error_prevr)/dt;
efvanmarrewijk 43:e8f2193822b7 173 float filtered_error_derivative = PIDfilterr.step(error_derivative);
efvanmarrewijk 30:c4a3e868ef04 174 float u_d = Kd * filtered_error_derivative;
efvanmarrewijk 43:e8f2193822b7 175 error_prevr = error;
efvanmarrewijk 27:3430dfb4c9fb 176 // Sum all parts and return it
efvanmarrewijk 30:c4a3e868ef04 177 return u_k + u_i + u_d;
efvanmarrewijk 27:3430dfb4c9fb 178 }
efvanmarrewijk 43:e8f2193822b7 179
efvanmarrewijk 43:e8f2193822b7 180 float DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 50:522bb6eebda6 181 { float angle = (pot2*2.0f*PI)-PI;
efvanmarrewijk 43:e8f2193822b7 182 return angle;
efvanmarrewijk 43:e8f2193822b7 183 }
efvanmarrewijk 30:c4a3e868ef04 184
efvanmarrewijk 43:e8f2193822b7 185 void turnr() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 26:b48708ed51ff 186 {
efvanmarrewijk 50:522bb6eebda6 187 //float refvaluer = PI/4.0f;
efvanmarrewijk 50:522bb6eebda6 188 float refvaluer = -DesiredAngler(); // DONT CHANGE THIS MINUS SIGN: different minus sign per motor
efvanmarrewijk 50:522bb6eebda6 189 int countsr = Countsrinput(); // different encoder pins per motor
efvanmarrewijk 50:522bb6eebda6 190 currentangler = CurrentAngle(countsr); // different minus sign per motor
efvanmarrewijk 50:522bb6eebda6 191 float PIDcontrolr = PIDcontrollerr(refvaluer,currentangler); // same for every motor
efvanmarrewijk 50:522bb6eebda6 192 errorr = ErrorCalc(refvaluer,currentangler); // same for every motor
efvanmarrewijk 30:c4a3e868ef04 193
efvanmarrewijk 50:522bb6eebda6 194 pin5 = fabs(PIDcontrolr); // different pins for every motor
efvanmarrewijk 50:522bb6eebda6 195 pin4 = PIDcontrolr > 0.0f; // different pins for every motor
efvanmarrewijk 35:ba556f2d0fcc 196 }
efvanmarrewijk 35:ba556f2d0fcc 197
efvanmarrewijk 43:e8f2193822b7 198 float PIDcontrollerf(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 50:522bb6eebda6 199 { //Kp = Kpcontr();
efvanmarrewijk 50:522bb6eebda6 200 float Kp = 19.24f;
efvanmarrewijk 50:522bb6eebda6 201 float Ki = 1.02f;
efvanmarrewijk 50:522bb6eebda6 202 float Kd = 0.827f;
efvanmarrewijk 50:522bb6eebda6 203 //Kd = Kdcontr();
efvanmarrewijk 43:e8f2193822b7 204 float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 43:e8f2193822b7 205 static float error_integralf = 0.0;
efvanmarrewijk 43:e8f2193822b7 206 static float error_prevf = error; // initialization with this value only done once!
efvanmarrewijk 43:e8f2193822b7 207 static BiQuad PIDfilterf(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 43:e8f2193822b7 208 // Proportional part:
efvanmarrewijk 43:e8f2193822b7 209 float u_k = Kp * error;
efvanmarrewijk 43:e8f2193822b7 210 // Integral part
efvanmarrewijk 43:e8f2193822b7 211 error_integralf = error_integralf + error * dt;
efvanmarrewijk 43:e8f2193822b7 212 float u_i = Ki * error_integralf;
efvanmarrewijk 43:e8f2193822b7 213 // Derivative part
efvanmarrewijk 43:e8f2193822b7 214 float error_derivative = (error - error_prevf)/dt;
efvanmarrewijk 43:e8f2193822b7 215 float filtered_error_derivative = PIDfilterf.step(error_derivative);
efvanmarrewijk 43:e8f2193822b7 216 float u_d = Kd * filtered_error_derivative;
efvanmarrewijk 43:e8f2193822b7 217 error_prevf = error;
efvanmarrewijk 43:e8f2193822b7 218 // Sum all parts and return it
efvanmarrewijk 43:e8f2193822b7 219 return u_k + u_i + u_d;
efvanmarrewijk 43:e8f2193822b7 220 }
efvanmarrewijk 35:ba556f2d0fcc 221
efvanmarrewijk 43:e8f2193822b7 222 float DesiredAnglef() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 50:522bb6eebda6 223 { float angle = (pot2*2.0f*PI)-PI;
efvanmarrewijk 43:e8f2193822b7 224 return angle;
efvanmarrewijk 43:e8f2193822b7 225 }
efvanmarrewijk 43:e8f2193822b7 226
efvanmarrewijk 43:e8f2193822b7 227 void turnf() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 43:e8f2193822b7 228 {
efvanmarrewijk 50:522bb6eebda6 229 float refvaluef = 0.0f;
efvanmarrewijk 50:522bb6eebda6 230 //float refvaluef = -DesiredAnglef(); // DONT CHANGE THIS MINUS SIGN: different minus sign per motor
efvanmarrewijk 50:522bb6eebda6 231 int countsf = Countsfinput(); // different encoder pins per motor
efvanmarrewijk 50:522bb6eebda6 232 currentanglef = CurrentAngle(countsf); // different minus sign per motor
efvanmarrewijk 50:522bb6eebda6 233 float PIDcontrolf = PIDcontrollerf(refvaluef,currentanglef); // same for every motor
efvanmarrewijk 50:522bb6eebda6 234 errorf = ErrorCalc(refvaluef,currentanglef); // same for every motor
efvanmarrewijk 43:e8f2193822b7 235
efvanmarrewijk 50:522bb6eebda6 236 pin3 = fabs(PIDcontrolf); // different pins for every motor
efvanmarrewijk 50:522bb6eebda6 237 pin2 = PIDcontrolf > 0.0f; // different pins for every motor
efvanmarrewijk 25:76e9e5597416 238 }
efvanmarrewijk 11:3efd6a324f16 239
efvanmarrewijk 16:720365110953 240 // Main program
efvanmarrewijk 11:3efd6a324f16 241 int main()
efvanmarrewijk 18:ca084c362855 242 {
efvanmarrewijk 26:b48708ed51ff 243 pc.baud(115200);
efvanmarrewijk 32:a5b411833d1e 244 pin3.period_us(15); // If you give a period on one pin, c++ gives all pins this period
efvanmarrewijk 31:91ad5b188bd9 245
efvanmarrewijk 43:e8f2193822b7 246 motorl.attach(turnl,dt);
efvanmarrewijk 50:522bb6eebda6 247 motorr.attach(turnr,dt);
efvanmarrewijk 50:522bb6eebda6 248 //motorf.attach(turnf,dt); // DONT TURN THIS ON, UNLESS THE CALIBRATION MODE WORKS
efvanmarrewijk 33:ec07e11676ec 249
efvanmarrewijk 26:b48708ed51ff 250 emergencybutton.rise(Emergency); //If the button is pressed, stop program
efvanmarrewijk 37:c61d7768c18a 251
efvanmarrewijk 16:720365110953 252 while (true)
efvanmarrewijk 25:76e9e5597416 253 {
efvanmarrewijk 50:522bb6eebda6 254 pc.printf("angle l/r/f: \t %f \t\t %f \t\t %f \t\t error l/r/f: \t %f \t\t %f \t\t %f\r\n",currentanglel,currentangler,currentanglef,errorl,errorr,errorf);
efvanmarrewijk 49:48363ca21a15 255
efvanmarrewijk 50:522bb6eebda6 256 //pc.printf("Kp: %f \t\t Kd: %f \t\t angle: %f \t\t error: %f\r\n",Kp,Kd,currentanglel,errorl);
efvanmarrewijk 43:e8f2193822b7 257 wait(0.1f);
Ramonwaninge 3:d39285fdd103 258 }
efvanmarrewijk 30:c4a3e868ef04 259 }