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@44:de6b4eac5cb7, 2018-10-31 (annotated)

Committer:
efvanmarrewijk
Date:
Wed Oct 31 17:32:25 2018 +0000
Revision:
44:de6b4eac5cb7
Parent:
43:e8f2193822b7 
With doubles, works but prints wrong values

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 44:de6b4eac5cb7 46 const double pi = 3.14159265358979;
efvanmarrewijk 44:de6b4eac5cb7 47 double u3 = 0.0; // Normalised variable for the movement of motor 3
efvanmarrewijk 44:de6b4eac5cb7 48 const double fCountsRad = 4200.0;
efvanmarrewijk 44:de6b4eac5cb7 49 const double dt = 0.001;
efvanmarrewijk 16:720365110953 50
efvanmarrewijk 44:de6b4eac5cb7 51 double currentanglel;
efvanmarrewijk 44:de6b4eac5cb7 52 double errorl;
efvanmarrewijk 44:de6b4eac5cb7 53 double currentangler;
efvanmarrewijk 44:de6b4eac5cb7 54 double errorr;
efvanmarrewijk 44:de6b4eac5cb7 55 double currentanglef;
efvanmarrewijk 44:de6b4eac5cb7 56 double 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 44:de6b4eac5cb7 86 double CurrentAngle(double counts) // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
efvanmarrewijk 44:de6b4eac5cb7 87 { double angle = ((double)counts*2.0*pi)/fCountsRad;
efvanmarrewijk 44:de6b4eac5cb7 88 while (angle > 2.0*pi)
efvanmarrewijk 44:de6b4eac5cb7 89 { angle = angle-2.0*pi;
efvanmarrewijk 27:3430dfb4c9fb 90 }
efvanmarrewijk 44:de6b4eac5cb7 91 while (angle < -2.0*pi)
efvanmarrewijk 44:de6b4eac5cb7 92 { angle = angle+2.0*pi;
efvanmarrewijk 30:c4a3e868ef04 93 }
efvanmarrewijk 30:c4a3e868ef04 94 return angle;
efvanmarrewijk 27:3430dfb4c9fb 95 }
efvanmarrewijk 27:3430dfb4c9fb 96
efvanmarrewijk 44:de6b4eac5cb7 97 double ErrorCalc(double refvalue,double CurAngle) // Calculates the error of the system, based on the current angle and the reference value
efvanmarrewijk 44:de6b4eac5cb7 98 { double error = refvalue - CurAngle;
efvanmarrewijk 30:c4a3e868ef04 99 return error;
efvanmarrewijk 30:c4a3e868ef04 100 }
efvanmarrewijk 30:c4a3e868ef04 101
efvanmarrewijk 44:de6b4eac5cb7 102 double Kpcontr() // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
efvanmarrewijk 44:de6b4eac5cb7 103 { double Kp = 20.0*pot2;
efvanmarrewijk 30:c4a3e868ef04 104 return Kp;
efvanmarrewijk 30:c4a3e868ef04 105 }
efvanmarrewijk 30:c4a3e868ef04 106
efvanmarrewijk 44:de6b4eac5cb7 107 double Kdcontr() // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 44:de6b4eac5cb7 108 { double Kd = 0.25*pot1;
efvanmarrewijk 30:c4a3e868ef04 109 return Kd;
efvanmarrewijk 30:c4a3e868ef04 110 }
efvanmarrewijk 43:e8f2193822b7 111
efvanmarrewijk 44:de6b4eac5cb7 112 double PIDcontrollerl(double refvalue,double CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 44:de6b4eac5cb7 113 { //double Kp = Kpcontr();
efvanmarrewijk 44:de6b4eac5cb7 114 double Kp = 10.42;
efvanmarrewijk 44:de6b4eac5cb7 115 double Ki = 1.02;
efvanmarrewijk 44:de6b4eac5cb7 116 double Kd = 0.0493;
efvanmarrewijk 44:de6b4eac5cb7 117 //double Kd = Kdcontr();
efvanmarrewijk 44:de6b4eac5cb7 118 double error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 44:de6b4eac5cb7 119 static double error_integrall = 0.0;
efvanmarrewijk 44:de6b4eac5cb7 120 static double 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 44:de6b4eac5cb7 123 double u_k = Kp * error;
efvanmarrewijk 43:e8f2193822b7 124 // Integral part
efvanmarrewijk 43:e8f2193822b7 125 error_integrall = error_integrall + error * dt;
efvanmarrewijk 44:de6b4eac5cb7 126 double u_i = Ki * error_integrall;
efvanmarrewijk 43:e8f2193822b7 127 // Derivative part
efvanmarrewijk 44:de6b4eac5cb7 128 double error_derivative = (error - error_prevl)/dt;
efvanmarrewijk 44:de6b4eac5cb7 129 double filtered_error_derivative = PIDfilterl.step(error_derivative);
efvanmarrewijk 44:de6b4eac5cb7 130 double 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 44:de6b4eac5cb7 136 double DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 44:de6b4eac5cb7 137 { double angle = (pot1*2.0*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 44:de6b4eac5cb7 143 //double refvaluel = pi/4.0f;
efvanmarrewijk 44:de6b4eac5cb7 144 double 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 44:de6b4eac5cb7 147 double 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 44:de6b4eac5cb7 151 pin7 = PIDcontroll > 0.0; // different pins for every motor
efvanmarrewijk 43:e8f2193822b7 152 }
efvanmarrewijk 43:e8f2193822b7 153
efvanmarrewijk 44:de6b4eac5cb7 154 double PIDcontrollerr(double refvalue,double CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 44:de6b4eac5cb7 155 { //double Kp = Kpcontr();
efvanmarrewijk 44:de6b4eac5cb7 156 double Kp = 10.42;
efvanmarrewijk 44:de6b4eac5cb7 157 double Ki = 1.02;
efvanmarrewijk 44:de6b4eac5cb7 158 double Kd = 0.0493;
efvanmarrewijk 44:de6b4eac5cb7 159 //double Kd = Kdcontr();
efvanmarrewijk 44:de6b4eac5cb7 160 double error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 44:de6b4eac5cb7 161 static double error_integralr = 0.0;
efvanmarrewijk 44:de6b4eac5cb7 162 static double 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 44:de6b4eac5cb7 165 double u_k = Kp * error;
efvanmarrewijk 27:3430dfb4c9fb 166 // Integral part
efvanmarrewijk 43:e8f2193822b7 167 error_integralr = error_integralr + error * dt;
efvanmarrewijk 44:de6b4eac5cb7 168 double u_i = Ki * error_integralr;
efvanmarrewijk 30:c4a3e868ef04 169 // Derivative part
efvanmarrewijk 44:de6b4eac5cb7 170 double error_derivative = (error - error_prevr)/dt;
efvanmarrewijk 44:de6b4eac5cb7 171 double filtered_error_derivative = PIDfilterr.step(error_derivative);
efvanmarrewijk 44:de6b4eac5cb7 172 double 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 44:de6b4eac5cb7 178 double DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 44:de6b4eac5cb7 179 { double angle = (pot2*2.0*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 44:de6b4eac5cb7 185 double refvaluer = pi/4.0;
efvanmarrewijk 44:de6b4eac5cb7 186 //double 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 44:de6b4eac5cb7 189 double 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 44:de6b4eac5cb7 193 pin4 = PIDcontrolr > 0.0; // different pins for every motor
efvanmarrewijk 35:ba556f2d0fcc 194 }
efvanmarrewijk 35:ba556f2d0fcc 195
efvanmarrewijk 44:de6b4eac5cb7 196 double PIDcontrollerf(double refvalue,double CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 44:de6b4eac5cb7 197 { //double Kp = Kpcontr();
efvanmarrewijk 44:de6b4eac5cb7 198 double Kp = 10.42;
efvanmarrewijk 44:de6b4eac5cb7 199 double Ki = 1.02;
efvanmarrewijk 44:de6b4eac5cb7 200 double Kd = 0.0493;
efvanmarrewijk 44:de6b4eac5cb7 201 //double Kd = Kdcontr();
efvanmarrewijk 44:de6b4eac5cb7 202 double error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk 44:de6b4eac5cb7 203 static double error_integralf = 0.0;
efvanmarrewijk 44:de6b4eac5cb7 204 static double 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 44:de6b4eac5cb7 207 double u_k = Kp * error;
efvanmarrewijk 43:e8f2193822b7 208 // Integral part
efvanmarrewijk 43:e8f2193822b7 209 error_integralf = error_integralf + error * dt;
efvanmarrewijk 44:de6b4eac5cb7 210 double u_i = Ki * error_integralf;
efvanmarrewijk 43:e8f2193822b7 211 // Derivative part
efvanmarrewijk 44:de6b4eac5cb7 212 double error_derivative = (error - error_prevf)/dt;
efvanmarrewijk 44:de6b4eac5cb7 213 double filtered_error_derivative = PIDfilterf.step(error_derivative);
efvanmarrewijk 44:de6b4eac5cb7 214 double 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 44:de6b4eac5cb7 220 double DesiredAnglef() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 44:de6b4eac5cb7 221 { double angle = (pot2*2.0*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 44:de6b4eac5cb7 227 //double refvaluef = pi/4.0f;
efvanmarrewijk 44:de6b4eac5cb7 228 double 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 44:de6b4eac5cb7 231 double 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 44:de6b4eac5cb7 235 pin2 = PIDcontrolf > 0.0; // different pins for every motor
efvanmarrewijk 18:ca084c362855 236 }
efvanmarrewijk 25:76e9e5597416 237
efvanmarrewijk 44:de6b4eac5cb7 238 double 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 44:de6b4eac5cb7 239 { double 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 44:de6b4eac5cb7 258 pc.printf("angle l/r/d: \t %d \t\t %d \t\t %d \t\t error l/r/f: \t %d \t\t %d \t\t %d\r\n",currentanglel,currentangler,currentanglef,errorl,errorr,errorf);
efvanmarrewijk 44:de6b4eac5cb7 259 wait(0.1);
Ramonwaninge 3:d39285fdd103 260 }
efvanmarrewijk 30:c4a3e868ef04 261 }