Ramon Waninge / Mbed 2 deprecated Milestone1

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 mbed QEI biquadFilter HIDScope MODSERIAL

main.cpp@48:36cdeaac67c5, 2018-10-31 (annotated)

Committer:
efvanmarrewijk
Date:
Wed Oct 31 19:31:06 2018 +0000
Revision:
48:36cdeaac67c5
Parent:
43:e8f2193822b7
Child:
49:48363ca21a15
Good angles of the motors: minus sign in code (please dont delete this sign!)

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 32:a5b411833d1e 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 32:a5b411833d1e 49 const float dt = 0.001f;
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 43:e8f2193822b7 57
efvanmarrewijk 16:720365110953 58 // Functions
efvanmarrewijk 32:a5b411833d1e 59 void Emergency() // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
efvanmarrewijk 32:a5b411833d1e 60 { greenled = 1; // Red light on
efvanmarrewijk 30:c4a3e868ef04 61 blueled = 1;
efvanmarrewijk 30:c4a3e868ef04 62 redled = 0;
efvanmarrewijk 32:a5b411833d1e 63 pin3 = 0; // All motor forces to zero
efvanmarrewijk 30:c4a3e868ef04 64 pin5 = 0;
efvanmarrewijk 30:c4a3e868ef04 65 pin6 = 0;
efvanmarrewijk 32:a5b411833d1e 66 exit (0); // Abort mission!!
efvanmarrewijk 30:c4a3e868ef04 67 }
efvanmarrewijk 30:c4a3e868ef04 68
efvanmarrewijk 31:91ad5b188bd9 69 // Subfunctions
efvanmarrewijk 35:ba556f2d0fcc 70 int Countslinput() // Gets the counts from encoder 1
efvanmarrewijk 35:ba556f2d0fcc 71 { int countsl;
efvanmarrewijk 35:ba556f2d0fcc 72 countsl = Encoderl.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 73 return countsl;
efvanmarrewijk 27:3430dfb4c9fb 74 }
efvanmarrewijk 35:ba556f2d0fcc 75 int Countsrinput() // Gets the counts from encoder 2
efvanmarrewijk 35:ba556f2d0fcc 76 { int countsr;
efvanmarrewijk 35:ba556f2d0fcc 77 countsr = Encoderr.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 78 return countsr;
efvanmarrewijk 27:3430dfb4c9fb 79 }
efvanmarrewijk 35:ba556f2d0fcc 80 int Countsfinput() // Gets the counts from encoder 3
efvanmarrewijk 35:ba556f2d0fcc 81 { int countsf;
efvanmarrewijk 35:ba556f2d0fcc 82 countsf = Encoderf.getPulses();
efvanmarrewijk 35:ba556f2d0fcc 83 return countsf;
efvanmarrewijk 27:3430dfb4c9fb 84 }
efvanmarrewijk 27:3430dfb4c9fb 85
efvanmarrewijk 32:a5b411833d1e 86 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 32:a5b411833d1e 87 { float angle = ((float)counts*2.0f*pi)/fCountsRad;
efvanmarrewijk 32:a5b411833d1e 88 while (angle > 2.0f*pi)
efvanmarrewijk 32:a5b411833d1e 89 { angle = angle-2.0f*pi;
efvanmarrewijk 27:3430dfb4c9fb 90 }
efvanmarrewijk 32:a5b411833d1e 91 while (angle < -2.0f*pi)
efvanmarrewijk 32:a5b411833d1e 92 { angle = angle+2.0f*pi;
efvanmarrewijk 30:c4a3e868ef04 93 }
efvanmarrewijk 30:c4a3e868ef04 94 return angle;
efvanmarrewijk 27:3430dfb4c9fb 95 }
efvanmarrewijk 27:3430dfb4c9fb 96
efvanmarrewijk 32:a5b411833d1e 97 float ErrorCalc(float refvalue,float CurAngle) // Calculates the error of the system, based on the current angle and the reference value
efvanmarrewijk 32:a5b411833d1e 98 { float error = refvalue - CurAngle;
efvanmarrewijk 30:c4a3e868ef04 99 return error;
efvanmarrewijk 30:c4a3e868ef04 100 }
efvanmarrewijk 30:c4a3e868ef04 101
efvanmarrewijk 32:a5b411833d1e 102 float Kpcontr() // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
efvanmarrewijk 32:a5b411833d1e 103 { float Kp = 20.0f*pot2;
efvanmarrewijk 30:c4a3e868ef04 104 return Kp;
efvanmarrewijk 30:c4a3e868ef04 105 }
efvanmarrewijk 30:c4a3e868ef04 106
efvanmarrewijk 32:a5b411833d1e 107 float Kdcontr() // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 32:a5b411833d1e 108 { float Kd = 0.25f*pot1;
efvanmarrewijk 30:c4a3e868ef04 109 return Kd;
efvanmarrewijk 30:c4a3e868ef04 110 }
efvanmarrewijk 43:e8f2193822b7 111
efvanmarrewijk 43:e8f2193822b7 112 float PIDcontrollerl(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 43:e8f2193822b7 113 { //float Kp = Kpcontr();
efvanmarrewijk 43:e8f2193822b7 114 float Kp = 10.42f;
efvanmarrewijk 43:e8f2193822b7 115 float Ki = 1.02f;
efvanmarrewijk 43:e8f2193822b7 116 float Kd = 0.0493f;
efvanmarrewijk 43:e8f2193822b7 117 //float Kd = Kdcontr();
efvanmarrewijk 43:e8f2193822b7 118 float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 43:e8f2193822b7 119 static float error_integrall = 0.0;
efvanmarrewijk 43:e8f2193822b7 120 static float error_prevl = error; // initialization with this value only done once!
efvanmarrewijk 43:e8f2193822b7 121 static BiQuad PIDfilterl(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 43:e8f2193822b7 122 // Proportional part:
efvanmarrewijk 43:e8f2193822b7 123 float u_k = Kp * error;
efvanmarrewijk 43:e8f2193822b7 124 // Integral part
efvanmarrewijk 43:e8f2193822b7 125 error_integrall = error_integrall + error * dt;
efvanmarrewijk 43:e8f2193822b7 126 float u_i = Ki * error_integrall;
efvanmarrewijk 43:e8f2193822b7 127 // Derivative part
efvanmarrewijk 43:e8f2193822b7 128 float error_derivative = (error - error_prevl)/dt;
efvanmarrewijk 43:e8f2193822b7 129 float filtered_error_derivative = PIDfilterl.step(error_derivative);
efvanmarrewijk 43:e8f2193822b7 130 float u_d = Kd * filtered_error_derivative;
efvanmarrewijk 43:e8f2193822b7 131 error_prevl = error;
efvanmarrewijk 43:e8f2193822b7 132 // Sum all parts and return it
efvanmarrewijk 43:e8f2193822b7 133 return u_k + u_i + u_d;
efvanmarrewijk 43:e8f2193822b7 134 }
efvanmarrewijk 43:e8f2193822b7 135
efvanmarrewijk 43:e8f2193822b7 136 float DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 43:e8f2193822b7 137 { float angle = (pot1*2.0f*pi)-pi;
efvanmarrewijk 43:e8f2193822b7 138 return angle;
efvanmarrewijk 43:e8f2193822b7 139 }
efvanmarrewijk 43:e8f2193822b7 140
efvanmarrewijk 43:e8f2193822b7 141 void turnl() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 43:e8f2193822b7 142 {
efvanmarrewijk 43:e8f2193822b7 143 //float refvaluel = pi/4.0f;
efvanmarrewijk 43:e8f2193822b7 144 float refvaluel = DesiredAnglel(); // different minus sign per motor
efvanmarrewijk 43:e8f2193822b7 145 int countsl = Countslinput(); // different encoder pins per motor
efvanmarrewijk 43:e8f2193822b7 146 currentanglel = CurrentAngle(countsl); // different minus sign per motor
efvanmarrewijk 43:e8f2193822b7 147 float PIDcontroll = PIDcontrollerl(refvaluel,currentanglel); // same for every motor
efvanmarrewijk 43:e8f2193822b7 148 errorl = ErrorCalc(refvaluel,currentanglel); // same for every motor
efvanmarrewijk 43:e8f2193822b7 149
efvanmarrewijk 43:e8f2193822b7 150 pin6 = fabs(PIDcontroll); // different pins for every motor
efvanmarrewijk 43:e8f2193822b7 151 pin7 = PIDcontroll > 0.0f; // different pins for every motor
efvanmarrewijk 43:e8f2193822b7 152 }
efvanmarrewijk 43:e8f2193822b7 153
efvanmarrewijk 43:e8f2193822b7 154 float PIDcontrollerr(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 30:c4a3e868ef04 155 { //float Kp = Kpcontr();
efvanmarrewijk 39:dcf3e5019a63 156 float Kp = 10.42f;
efvanmarrewijk 32:a5b411833d1e 157 float Ki = 1.02f;
efvanmarrewijk 39:dcf3e5019a63 158 float Kd = 0.0493f;
efvanmarrewijk 30:c4a3e868ef04 159 //float Kd = Kdcontr();
efvanmarrewijk 32:a5b411833d1e 160 float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 43:e8f2193822b7 161 static float error_integralr = 0.0;
efvanmarrewijk 43:e8f2193822b7 162 static float error_prevr = error; // initialization with this value only done once!
efvanmarrewijk 43:e8f2193822b7 163 static BiQuad PIDfilterr(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 27:3430dfb4c9fb 164 // Proportional part:
efvanmarrewijk 30:c4a3e868ef04 165 float u_k = Kp * error;
efvanmarrewijk 27:3430dfb4c9fb 166 // Integral part
efvanmarrewijk 43:e8f2193822b7 167 error_integralr = error_integralr + error * dt;
efvanmarrewijk 43:e8f2193822b7 168 float u_i = Ki * error_integralr;
efvanmarrewijk 30:c4a3e868ef04 169 // Derivative part
efvanmarrewijk 43:e8f2193822b7 170 float error_derivative = (error - error_prevr)/dt;
efvanmarrewijk 43:e8f2193822b7 171 float filtered_error_derivative = PIDfilterr.step(error_derivative);
efvanmarrewijk 30:c4a3e868ef04 172 float u_d = Kd * filtered_error_derivative;
efvanmarrewijk 43:e8f2193822b7 173 error_prevr = error;
efvanmarrewijk 27:3430dfb4c9fb 174 // Sum all parts and return it
efvanmarrewijk 30:c4a3e868ef04 175 return u_k + u_i + u_d;
efvanmarrewijk 27:3430dfb4c9fb 176 }
efvanmarrewijk 43:e8f2193822b7 177
efvanmarrewijk 43:e8f2193822b7 178 float DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 43:e8f2193822b7 179 { float angle = (pot2*2.0f*pi)-pi;
efvanmarrewijk 43:e8f2193822b7 180 return angle;
efvanmarrewijk 43:e8f2193822b7 181 }
efvanmarrewijk 30:c4a3e868ef04 182
efvanmarrewijk 43:e8f2193822b7 183 void turnr() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 26:b48708ed51ff 184 {
efvanmarrewijk 43:e8f2193822b7 185 float refvaluer = pi/4.0f;
efvanmarrewijk 43:e8f2193822b7 186 //float refvaluer = DesiredAngler(); // different minus sign per motor
efvanmarrewijk 43:e8f2193822b7 187 int countsr = Countsrinput(); // different encoder pins per motor
efvanmarrewijk 43:e8f2193822b7 188 currentangler = CurrentAngle(countsr); // different minus sign per motor
efvanmarrewijk 43:e8f2193822b7 189 float PIDcontrolr = PIDcontrollerr(refvaluer,currentangler); // same for every motor
efvanmarrewijk 43:e8f2193822b7 190 errorr = ErrorCalc(refvaluer,currentangler); // same for every motor
efvanmarrewijk 30:c4a3e868ef04 191
efvanmarrewijk 43:e8f2193822b7 192 pin5 = fabs(PIDcontrolr); // different pins for every motor
efvanmarrewijk 43:e8f2193822b7 193 pin4 = PIDcontrolr > 0.0f; // different pins for every motor
efvanmarrewijk 35:ba556f2d0fcc 194 }
efvanmarrewijk 35:ba556f2d0fcc 195
efvanmarrewijk 43:e8f2193822b7 196 float PIDcontrollerf(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 43:e8f2193822b7 197 { //float Kp = Kpcontr();
efvanmarrewijk 43:e8f2193822b7 198 float Kp = 10.42f;
efvanmarrewijk 43:e8f2193822b7 199 float Ki = 1.02f;
efvanmarrewijk 43:e8f2193822b7 200 float Kd = 0.0493f;
efvanmarrewijk 43:e8f2193822b7 201 //float Kd = Kdcontr();
efvanmarrewijk 43:e8f2193822b7 202 float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 43:e8f2193822b7 203 static float error_integralf = 0.0;
efvanmarrewijk 43:e8f2193822b7 204 static float error_prevf = error; // initialization with this value only done once!
efvanmarrewijk 43:e8f2193822b7 205 static BiQuad PIDfilterf(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 43:e8f2193822b7 206 // Proportional part:
efvanmarrewijk 43:e8f2193822b7 207 float u_k = Kp * error;
efvanmarrewijk 43:e8f2193822b7 208 // Integral part
efvanmarrewijk 43:e8f2193822b7 209 error_integralf = error_integralf + error * dt;
efvanmarrewijk 43:e8f2193822b7 210 float u_i = Ki * error_integralf;
efvanmarrewijk 43:e8f2193822b7 211 // Derivative part
efvanmarrewijk 43:e8f2193822b7 212 float error_derivative = (error - error_prevf)/dt;
efvanmarrewijk 43:e8f2193822b7 213 float filtered_error_derivative = PIDfilterf.step(error_derivative);
efvanmarrewijk 43:e8f2193822b7 214 float u_d = Kd * filtered_error_derivative;
efvanmarrewijk 43:e8f2193822b7 215 error_prevf = error;
efvanmarrewijk 43:e8f2193822b7 216 // Sum all parts and return it
efvanmarrewijk 43:e8f2193822b7 217 return u_k + u_i + u_d;
efvanmarrewijk 43:e8f2193822b7 218 }
efvanmarrewijk 35:ba556f2d0fcc 219
efvanmarrewijk 43:e8f2193822b7 220 float DesiredAnglef() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 43:e8f2193822b7 221 { float angle = (pot2*2.0f*pi)-pi;
efvanmarrewijk 43:e8f2193822b7 222 return angle;
efvanmarrewijk 43:e8f2193822b7 223 }
efvanmarrewijk 43:e8f2193822b7 224
efvanmarrewijk 43:e8f2193822b7 225 void turnf() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 43:e8f2193822b7 226 {
efvanmarrewijk 43:e8f2193822b7 227 //float refvaluef = pi/4.0f;
efvanmarrewijk 48:36cdeaac67c5 228 float refvaluef = -DesiredAnglef(); // different minus sign per motor
efvanmarrewijk 43:e8f2193822b7 229 int countsf = Countsfinput(); // different encoder pins per motor
efvanmarrewijk 43:e8f2193822b7 230 currentanglef = CurrentAngle(countsf); // different minus sign per motor
efvanmarrewijk 43:e8f2193822b7 231 float PIDcontrolf = PIDcontrollerf(refvaluef,currentanglef); // same for every motor
efvanmarrewijk 43:e8f2193822b7 232 errorf = ErrorCalc(refvaluef,currentanglef); // same for every motor
efvanmarrewijk 43:e8f2193822b7 233
efvanmarrewijk 43:e8f2193822b7 234 pin3 = fabs(PIDcontrolf); // different pins for every motor
efvanmarrewijk 43:e8f2193822b7 235 pin2 = PIDcontrolf > 0.0f; // different pins for every motor
efvanmarrewijk 18:ca084c362855 236 }
efvanmarrewijk 25:76e9e5597416 237
efvanmarrewijk 32:a5b411833d1e 238 float ActualPosition(int counts, int offsetcounts) // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
efvanmarrewijk 32:a5b411833d1e 239 { float MotorPosition = - (counts - offsetcounts) / fCountsRad;
efvanmarrewijk 25:76e9e5597416 240 // minus sign to correct for direction convention
efvanmarrewijk 32:a5b411833d1e 241 return MotorPosition;
efvanmarrewijk 25:76e9e5597416 242 }
efvanmarrewijk 11:3efd6a324f16 243
efvanmarrewijk 16:720365110953 244 // Main program
efvanmarrewijk 11:3efd6a324f16 245 int main()
efvanmarrewijk 18:ca084c362855 246 {
efvanmarrewijk 26:b48708ed51ff 247 pc.baud(115200);
efvanmarrewijk 32:a5b411833d1e 248 pin3.period_us(15); // If you give a period on one pin, c++ gives all pins this period
efvanmarrewijk 31:91ad5b188bd9 249
efvanmarrewijk 43:e8f2193822b7 250 motorl.attach(turnl,dt);
efvanmarrewijk 43:e8f2193822b7 251 motorr.attach(turnr,dt);
efvanmarrewijk 43:e8f2193822b7 252 motorf.attach(turnf,dt);
efvanmarrewijk 33:ec07e11676ec 253
efvanmarrewijk 26:b48708ed51ff 254 emergencybutton.rise(Emergency); //If the button is pressed, stop program
efvanmarrewijk 37:c61d7768c18a 255
efvanmarrewijk 16:720365110953 256 while (true)
efvanmarrewijk 25:76e9e5597416 257 {
efvanmarrewijk 43:e8f2193822b7 258 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 43:e8f2193822b7 259 wait(0.1f);
Ramonwaninge 3:d39285fdd103 260 }
efvanmarrewijk 30:c4a3e868ef04 261 }