Tim Luten / Mbed 2 deprecated DEMO_TEST_LIJN

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:   Demo_TEST3 QEI biquadFilter mbed

Fork of Demo_TEST3 by Biorobotica TIC

main.cpp@5:e7253884c2e4, 2018-10-24 (annotated)

Committer:
Hubertus
Date:
Wed Oct 24 12:09:27 2018 +0000
Revision:
5:e7253884c2e4
Parent:
4:5ceb8f058874 
3 EMG Signalen gekoppeld aan de 3 motoren, met 2 knopjes voor de richting. + encoder en printen naar de computer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hubertus 0:df553b18547d 1 #include "mbed.h"
Hubertus 0:df553b18547d 2 #include "math.h"
Hubertus 0:df553b18547d 3 #include "BiQuad.h"
Hubertus 5:e7253884c2e4 4 #include "Servo.h"
Hubertus 5:e7253884c2e4 5 #include "QEI.h"
Hubertus 5:e7253884c2e4 6 //#include "HIDScope.h"
Hubertus 0:df553b18547d 7
Hubertus 1:5c3259ecf10a 8
Hubertus 3:be5ac89a0b53 9
Hubertus 0:df553b18547d 10 //Define objects
Hubertus 4:5ceb8f058874 11 AnalogIn emgL(A0); // EMG Left Arm
Hubertus 4:5ceb8f058874 12 AnalogIn emgR(A1); // EMG Right Arm
Hubertus 4:5ceb8f058874 13 AnalogIn emgS(A2); // EMG Servo spier
Hubertus 0:df553b18547d 14 DigitalOut ledB(LED_BLUE); // Informative LED for gaining zero and max
Hubertus 1:5c3259ecf10a 15 DigitalOut ledR(LED_RED);
Hubertus 0:df553b18547d 16 DigitalOut ledG(LED_GREEN); // Informative LED for gaining zero and max
Hubertus 4:5ceb8f058874 17 DigitalIn CalButton(PTA4); // Button used for gaining zero and max
Hubertus 4:5ceb8f058874 18 DigitalIn zeromax(PTC6); // Button used for switching between zero and max
Hubertus 0:df553b18547d 19 Ticker emgSampleTicker; // Ticker for sample frequency
Hubertus 5:e7253884c2e4 20 //HIDScope scope( 6 );
Hubertus 5:e7253884c2e4 21 Servo myservo(D8);
Hubertus 3:be5ac89a0b53 22 DigitalOut motor1direction(D7);
Hubertus 3:be5ac89a0b53 23 PwmOut motor1control(D6);
Hubertus 5:e7253884c2e4 24 DigitalOut motor2direction(D4);
Hubertus 5:e7253884c2e4 25 PwmOut motor2control(D5);
Hubertus 5:e7253884c2e4 26 InterruptIn button1(D10);
Hubertus 5:e7253884c2e4 27 InterruptIn button2(D11);
Hubertus 5:e7253884c2e4 28
Hubertus 5:e7253884c2e4 29 QEI EncoderL(D12,D13,NC,64,QEI::X2_ENCODING);
Hubertus 5:e7253884c2e4 30 QEI EncoderR(D2,D3,NC,64,QEI::X2_ENCODING);
Hubertus 5:e7253884c2e4 31
Hubertus 5:e7253884c2e4 32 Ticker PrintTicker;
Hubertus 5:e7253884c2e4 33 Serial pc(USBTX, USBRX);
Hubertus 0:df553b18547d 34
Hubertus 0:df553b18547d 35
Hubertus 4:5ceb8f058874 36 int P= 200; // Number of points for movag first emg
Hubertus 4:5ceb8f058874 37 int Q = 200; // Number of points for movag second emg
Hubertus 4:5ceb8f058874 38 int R = 200; // Number of points for movag third emg
Hubertus 0:df553b18547d 39 double A[200]; // Vector for storing data of first emg
Hubertus 0:df553b18547d 40 double B[200]; // Vector for storing data of second emg
Hubertus 4:5ceb8f058874 41 double C[200]; // Vector for storing data of third emg
Hubertus 0:df553b18547d 42 int k = 0; // Counter for configuration:
Hubertus 4:5ceb8f058874 43 double Lvector[200]; // Vector for Rwaarde configuration
Hubertus 4:5ceb8f058874 44 double Lwaarde[2]; // Vector for storage of max and zero of left biceps
Hubertus 4:5ceb8f058874 45 double Rvector[200]; // Vector for Lwaarde configuration
Hubertus 4:5ceb8f058874 46 double Rwaarde[2]; // Vector for storage of max and zero of right biceps
Hubertus 4:5ceb8f058874 47 double Svector[200]; // Vector for Swaarde configuration
Hubertus 4:5ceb8f058874 48 double Swaarde[2]; // Vector for storage of max and zero of servo emg
Hubertus 0:df553b18547d 49 int x = 0; // Variable for switching between zero and max
Hubertus 4:5ceb8f058874 50 double movagR; // Moving Average mean value of right biceps
Hubertus 4:5ceb8f058874 51 double movagL; // Moving Average mean value of left biceps
Hubertus 4:5ceb8f058874 52 double movagS; // Moving Average mean value of servo spier
Hubertus 0:df553b18547d 53 float thresholdL = 10; // Startvalue for threshold, to block signals -
Hubertus 0:df553b18547d 54 float thresholdR = 10; // - before the zero and max values are calculated
Hubertus 4:5ceb8f058874 55 float thresholdS = 10; //-------
Hubertus 5:e7253884c2e4 56 const bool clockwise1 = true;
Hubertus 5:e7253884c2e4 57 const bool clockwise2 = true;
Hubertus 5:e7253884c2e4 58 volatile bool direction1 = clockwise1;
Hubertus 5:e7253884c2e4 59 volatile bool direction2 = clockwise2;
Hubertus 0:df553b18547d 60
Hubertus 4:5ceb8f058874 61 //------------Filter parameters----------------------
Hubertus 4:5ceb8f058874 62
Hubertus 4:5ceb8f058874 63 //Lowpassfilter
Hubertus 4:5ceb8f058874 64 //const double b0LP = 0.0014831498359569692;
Hubertus 4:5ceb8f058874 65 //const double b1LP = 0.0029662996719139385;
Hubertus 4:5ceb8f058874 66 //const double b2LP = 0.0014831498359569692;
Hubertus 4:5ceb8f058874 67 //const double a1LP = -1.918570032544273;
Hubertus 4:5ceb8f058874 68 //const double a2LP = 0.9245026318881009;
Hubertus 4:5ceb8f058874 69 //Highpassfilter Fc = 10 Hz;, Q = 0.5, Fs = 500 Hz
Hubertus 4:5ceb8f058874 70 const double b0HP = 0.8851221317817073;
Hubertus 4:5ceb8f058874 71 const double b1HP = -1.7702442635634146;
Hubertus 4:5ceb8f058874 72 const double b2HP = 0.8851221317817073;
Hubertus 4:5ceb8f058874 73 const double a1HP = -1.7632371847263784;
Hubertus 4:5ceb8f058874 74 const double a2HP = 0.777251342400451;
Hubertus 4:5ceb8f058874 75 //Notchfilter Fc = 50 Hz, Q = 10, Fs = 500 Hz
Hubertus 4:5ceb8f058874 76 const double b0NO = 0.9714498065192796;
Hubertus 4:5ceb8f058874 77 const double b1NO = -1.5718388053127037;
Hubertus 4:5ceb8f058874 78 const double b2NO = 0.9714498065192796;
Hubertus 4:5ceb8f058874 79 const double a1NO = -1.5718388053127037;
Hubertus 4:5ceb8f058874 80 const double a2NO = 0.9428996130385592;
Hubertus 4:5ceb8f058874 81
Hubertus 4:5ceb8f058874 82 //--------------Filter------------
Hubertus 4:5ceb8f058874 83 //BiQuad LPR( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
Hubertus 4:5ceb8f058874 84 BiQuad HPR( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
Hubertus 4:5ceb8f058874 85 BiQuad NOR( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
Hubertus 4:5ceb8f058874 86
Hubertus 4:5ceb8f058874 87 //BiQuad LPL( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
Hubertus 4:5ceb8f058874 88 BiQuad HPL( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
Hubertus 4:5ceb8f058874 89 BiQuad NOL( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
Hubertus 4:5ceb8f058874 90
Hubertus 4:5ceb8f058874 91 //BiQuad LPS( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
Hubertus 4:5ceb8f058874 92 BiQuad HPS( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
Hubertus 4:5ceb8f058874 93 BiQuad NOS( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
Hubertus 0:df553b18547d 94
Hubertus 0:df553b18547d 95
Hubertus 0:df553b18547d 96 void emgSample() { // EMG function
Hubertus 4:5ceb8f058874 97
Hubertus 4:5ceb8f058874 98 double emgNOFilteredL = NOL.step(emgL.read()); // Filtered NO value of EMG signal left biceps
Hubertus 4:5ceb8f058874 99 double emgHPFilteredL = HPL.step(emgNOFilteredL); // Filtered HP value of EMG signal left biceps
Hubertus 4:5ceb8f058874 100 double emgabsL = fabs(emgHPFilteredL); // Absolute value of EMG signal left biceps
Hubertus 4:5ceb8f058874 101
Hubertus 3:be5ac89a0b53 102
Hubertus 4:5ceb8f058874 103 double emgNOFilteredR = NOR.step(emgR.read()); // Filtered NO value of EMG signal right biceps
Hubertus 4:5ceb8f058874 104 double emgHPFilteredR = HPR.step(emgNOFilteredR); // Filtered HP value of EMG signal right biceps
Hubertus 4:5ceb8f058874 105 double emgabsR = fabs(emgHPFilteredR); // Absolute value of EMG signal right biceps
Hubertus 1:5c3259ecf10a 106
Hubertus 4:5ceb8f058874 107
Hubertus 4:5ceb8f058874 108 double emgNOFilteredS = NOS.step(emgS.read()); // Filtered NO value of EMG signal servo spier
Hubertus 4:5ceb8f058874 109 double emgHPFilteredS = HPS.step(emgNOFilteredS); // Filtered HP value of EMG signal servo spier
Hubertus 4:5ceb8f058874 110 double emgabsS = fabs(emgHPFilteredS); // Absolute value of EMG signal servo spier
Hubertus 2:36ad60c0aa01 111
Hubertus 1:5c3259ecf10a 112
Hubertus 0:df553b18547d 113
Hubertus 0:df553b18547d 114
Hubertus 4:5ceb8f058874 115 //-------------------Linker bicep-------------------------------------
Hubertus 4:5ceb8f058874 116
Hubertus 0:df553b18547d 117 for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 0:df553b18547d 118 if (i == 0) {
Hubertus 2:36ad60c0aa01 119 A[i] = emgabsL;
Hubertus 0:df553b18547d 120 }
Hubertus 0:df553b18547d 121 else {
Hubertus 0:df553b18547d 122 A[i] = A[i-1];
Hubertus 0:df553b18547d 123 }
Hubertus 0:df553b18547d 124 }
Hubertus 0:df553b18547d 125 double sumL = 0;
Hubertus 0:df553b18547d 126 for (int n = 0; n < P-1; n++) { // Summation of array
Hubertus 0:df553b18547d 127 sumL = sumL + A[n];
Hubertus 0:df553b18547d 128 }
Hubertus 4:5ceb8f058874 129 movagL = sumL/P;
Hubertus 4:5ceb8f058874 130
Hubertus 0:df553b18547d 131
Hubertus 0:df553b18547d 132
Hubertus 4:5ceb8f058874 133 //--------------Rechter bicep---------------------------------------
Hubertus 4:5ceb8f058874 134
Hubertus 4:5ceb8f058874 135 for(int i = Q-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 0:df553b18547d 136 if (i == 0) {
Hubertus 0:df553b18547d 137 B[i] = emgabsR;
Hubertus 0:df553b18547d 138 }
Hubertus 0:df553b18547d 139 else {
Hubertus 0:df553b18547d 140 B[i] = B[i-1];
Hubertus 0:df553b18547d 141 }
Hubertus 0:df553b18547d 142 }
Hubertus 0:df553b18547d 143 double sumR = 0;
Hubertus 0:df553b18547d 144
Hubertus 4:5ceb8f058874 145 for (int n = 0; n < Q-1; n++) { // Summation of array
Hubertus 0:df553b18547d 146 sumR = sumR + B[n];
Hubertus 0:df553b18547d 147 }
Hubertus 4:5ceb8f058874 148 movagR = sumR/Q; // Moving average value right biceps
Hubertus 4:5ceb8f058874 149
Hubertus 4:5ceb8f058874 150 //---------------Servo spier ------------------------------------
Hubertus 4:5ceb8f058874 151
Hubertus 4:5ceb8f058874 152 for(int i = R-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 4:5ceb8f058874 153 if (i == 0) {
Hubertus 4:5ceb8f058874 154 C[i] = emgabsS;
Hubertus 4:5ceb8f058874 155 }
Hubertus 4:5ceb8f058874 156 else {
Hubertus 4:5ceb8f058874 157 C[i] = C[i-1];
Hubertus 4:5ceb8f058874 158 }
Hubertus 4:5ceb8f058874 159 }
Hubertus 4:5ceb8f058874 160 double sumS = 0;
Hubertus 4:5ceb8f058874 161 for (int n = 0; n < R-1; n++) { // Summation of array
Hubertus 4:5ceb8f058874 162 sumS = sumS + C[n];
Hubertus 4:5ceb8f058874 163 }
Hubertus 4:5ceb8f058874 164 movagS = sumS/R;
Hubertus 3:be5ac89a0b53 165
Hubertus 3:be5ac89a0b53 166 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
Hubertus 5:e7253884c2e4 167 //scope.set(0, emgL.read());
Hubertus 5:e7253884c2e4 168 // scope.set(1, movagL);
Hubertus 5:e7253884c2e4 169 // scope.set(2, emgR.read());
Hubertus 5:e7253884c2e4 170 // scope.set(3, movagR);
Hubertus 5:e7253884c2e4 171 // scope.set(4, emgS.read());
Hubertus 5:e7253884c2e4 172 // scope.set(5, movagS);
Hubertus 3:be5ac89a0b53 173 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
Hubertus 3:be5ac89a0b53 174 * Ensure that enough channels are available (HIDScope scope( 2 ))
Hubertus 3:be5ac89a0b53 175 * Finally, send all channels to the PC at once */
Hubertus 5:e7253884c2e4 176 //scope.send(); // Moving average value left biceps
Hubertus 0:df553b18547d 177
Hubertus 0:df553b18547d 178
Hubertus 4:5ceb8f058874 179 if (CalButton==0 & k<200) { // Loop used for gaining max and zero value
Hubertus 4:5ceb8f058874 180 Lvector[k] = movagL;
Hubertus 4:5ceb8f058874 181 Rvector[k] = movagR;
Hubertus 4:5ceb8f058874 182 Svector[k] = movagS;
Hubertus 3:be5ac89a0b53 183
Hubertus 1:5c3259ecf10a 184 if (x==0){
Hubertus 1:5c3259ecf10a 185 ledB = !ledB; // SPIER NIET AANSPANNEN BIJ BLAUW
Hubertus 1:5c3259ecf10a 186 } // SPIER WEL AANSPANNEN BIJ ROOD
Hubertus 1:5c3259ecf10a 187 else if (x==1){
Hubertus 1:5c3259ecf10a 188 ledR = !ledR;
Hubertus 1:5c3259ecf10a 189 }
Hubertus 0:df553b18547d 190 k++;
Hubertus 0:df553b18547d 191 }
Hubertus 0:df553b18547d 192 else if (k==200) { // End of the loop, used for calculation value
Hubertus 0:df553b18547d 193 double sumY = 0;
Hubertus 0:df553b18547d 194 double sumZ = 0;
Hubertus 4:5ceb8f058874 195 double sumX = 0;
Hubertus 0:df553b18547d 196 for (int n = 0; n < 199; n++) {
Hubertus 0:df553b18547d 197 sumY = sumY + Lvector[n];
Hubertus 0:df553b18547d 198 sumZ = sumZ + Rvector[n];
Hubertus 4:5ceb8f058874 199 sumX = sumX + Svector[n];
Hubertus 0:df553b18547d 200 }
Hubertus 0:df553b18547d 201 Lwaarde[x] = sumY/200; // Value of zero for left biceps
Hubertus 4:5ceb8f058874 202 Rwaarde[x] = sumZ/200; // Value of zero for right biceps
Hubertus 4:5ceb8f058874 203 Swaarde[x] = sumX/200; // Value of zero for Servo spier
Hubertus 0:df553b18547d 204 k++;
Hubertus 0:df553b18547d 205 ledB = 1;
Hubertus 1:5c3259ecf10a 206 ledR = 1;
Hubertus 0:df553b18547d 207 ledG = !ledG;
Hubertus 0:df553b18547d 208 }
Hubertus 3:be5ac89a0b53 209 else if (k == 201 && zeromax ==0) { // Loop used for switching between zero and max
Hubertus 0:df553b18547d 210 ledG = !ledG;
Hubertus 0:df553b18547d 211 k = 0;
Hubertus 0:df553b18547d 212 if (x==0) {
Hubertus 0:df553b18547d 213 x++;
Hubertus 0:df553b18547d 214 }
Hubertus 0:df553b18547d 215 else if (x==1) {
Hubertus 0:df553b18547d 216 x=0;
Hubertus 0:df553b18547d 217 }
Hubertus 0:df553b18547d 218 }
Hubertus 0:df553b18547d 219 if (x==1) // Determining threshold using zero and max
Hubertus 0:df553b18547d 220 {
Hubertus 0:df553b18547d 221 thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
Hubertus 0:df553b18547d 222 thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
Hubertus 4:5ceb8f058874 223 thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
Hubertus 5:e7253884c2e4 224 }
Hubertus 5:e7253884c2e4 225 }
Hubertus 5:e7253884c2e4 226
Hubertus 5:e7253884c2e4 227
Hubertus 5:e7253884c2e4 228 //--------------------------Directions motor 1 aanpassen-------------------------
Hubertus 5:e7253884c2e4 229 void onButtonPress1() {
Hubertus 5:e7253884c2e4 230 // reverses the direction
Hubertus 5:e7253884c2e4 231 motor1direction.write(direction1= !direction1);
Hubertus 5:e7253884c2e4 232 pc.printf("direction1: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
Hubertus 5:e7253884c2e4 233 }
Hubertus 5:e7253884c2e4 234
Hubertus 5:e7253884c2e4 235 //--------------------------Directions motor 2 aanpassen-------------------------
Hubertus 5:e7253884c2e4 236 void onButtonPress2() {
Hubertus 5:e7253884c2e4 237 // reverses the direction
Hubertus 5:e7253884c2e4 238 motor2direction.write(direction2= !direction2);
Hubertus 5:e7253884c2e4 239 pc.printf("direction2: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
Hubertus 5:e7253884c2e4 240 }
Hubertus 5:e7253884c2e4 241
Hubertus 5:e7253884c2e4 242 //-----------------------Print functie----------------------------------------------
Hubertus 5:e7253884c2e4 243 void Printen() {
Hubertus 5:e7253884c2e4 244
Hubertus 5:e7253884c2e4 245 const float pi = 3.141592653589793; // Value of pi
Hubertus 5:e7253884c2e4 246 double gearratio = 3.857142857;
Hubertus 5:e7253884c2e4 247 double radiuspulley = 15.915;
Hubertus 5:e7253884c2e4 248 double hoekgraad = EncoderL.getPulses() * 0.0857142857;
Hubertus 5:e7253884c2e4 249 double hoekrad = hoekgraad * 0.0174532925;
Hubertus 5:e7253884c2e4 250 double hoekgraad2 = EncoderR.getPulses() * 0.0857142857;
Hubertus 5:e7253884c2e4 251 double hoekrad2 = hoekgraad2 * 0.0174532925;
Hubertus 5:e7253884c2e4 252 double hoekarm = hoekgraad2 / gearratio;
Hubertus 5:e7253884c2e4 253 double translatie = hoekgraad /360 * 2 * pi * radiuspulley;
Hubertus 5:e7253884c2e4 254
Hubertus 5:e7253884c2e4 255
Hubertus 5:e7253884c2e4 256 pc.printf("counts :%i \r\n", EncoderL.getPulses());
Hubertus 5:e7253884c2e4 257 pc.printf("hoekgraad :%f \r\n", hoekgraad );
Hubertus 5:e7253884c2e4 258 pc.printf("hoekrad :%f \r\n", hoekrad );
Hubertus 5:e7253884c2e4 259 pc.printf("translatie :%f mm \r\n", translatie );
Hubertus 5:e7253884c2e4 260 pc.printf("\n");
Hubertus 5:e7253884c2e4 261
Hubertus 5:e7253884c2e4 262
Hubertus 5:e7253884c2e4 263 pc.printf("counts2 :%i \r\n", EncoderR.getPulses());
Hubertus 5:e7253884c2e4 264 pc.printf("hoekgraad2 :%f \r\n", hoekgraad2 );
Hubertus 5:e7253884c2e4 265 pc.printf("hoekrad2 :%f \r\n", hoekrad2 );
Hubertus 5:e7253884c2e4 266 pc.printf("hoekarm :%f \r\n", hoekarm );
Hubertus 5:e7253884c2e4 267 pc.printf("\n");
Hubertus 5:e7253884c2e4 268
Hubertus 0:df553b18547d 269 }
Hubertus 0:df553b18547d 270
Hubertus 0:df553b18547d 271 int main()
Hubertus 0:df553b18547d 272 {
Hubertus 5:e7253884c2e4 273 //------------------EMG samplen-----------------------------
Hubertus 0:df553b18547d 274 ledB = 1;
Hubertus 0:df553b18547d 275 ledG = 1;
Hubertus 1:5c3259ecf10a 276 ledR = 1;
Hubertus 0:df553b18547d 277 emgSampleTicker.attach( &emgSample, 0.002 ); // Ticker for EMG function
Hubertus 0:df553b18547d 278 Lwaarde[0] = 0.01; // Startvalue for zerovalue, to -
Hubertus 0:df553b18547d 279 Rwaarde[0] = 0.01; // - prevent dividing by 0
Hubertus 4:5ceb8f058874 280 Swaarde[0] = 0.01;
Hubertus 5:e7253884c2e4 281
Hubertus 5:e7253884c2e4 282 //-----------------Motor button directions----------------------------
Hubertus 5:e7253884c2e4 283 pc.baud(115200);
Hubertus 5:e7253884c2e4 284
Hubertus 5:e7253884c2e4 285 button1.fall(&onButtonPress1);
Hubertus 5:e7253884c2e4 286 button2.fall(&onButtonPress2);
Hubertus 5:e7253884c2e4 287 PrintTicker.attach(&Printen, 1);
Hubertus 5:e7253884c2e4 288
Hubertus 5:e7253884c2e4 289
Hubertus 5:e7253884c2e4 290 //------------------Oneindige loop--------------------------------------
Hubertus 0:df553b18547d 291 while(1) {
Hubertus 3:be5ac89a0b53 292
Hubertus 5:e7253884c2e4 293 if (movagL > thresholdL)
Hubertus 5:e7253884c2e4 294 { motor1control.write(0.8);
Hubertus 5:e7253884c2e4 295 motor1direction.write(true);
Hubertus 5:e7253884c2e4 296 }
Hubertus 5:e7253884c2e4 297 else {
Hubertus 5:e7253884c2e4 298 motor1control.write(0);
Hubertus 5:e7253884c2e4 299 }
Hubertus 5:e7253884c2e4 300
Hubertus 5:e7253884c2e4 301 if (movagR > thresholdR)
Hubertus 5:e7253884c2e4 302 { motor2control.write(0.8);
Hubertus 5:e7253884c2e4 303 motor2direction.write(true);
Hubertus 5:e7253884c2e4 304 }
Hubertus 5:e7253884c2e4 305 else {
Hubertus 5:e7253884c2e4 306 motor1control.write(0);
Hubertus 5:e7253884c2e4 307 }
Hubertus 5:e7253884c2e4 308
Hubertus 5:e7253884c2e4 309
Hubertus 5:e7253884c2e4 310 if (movagS > thresholdS)
Hubertus 5:e7253884c2e4 311 { myservo = 0.5;
Hubertus 5:e7253884c2e4 312 wait(0.01);
Hubertus 5:e7253884c2e4 313 }
Hubertus 5:e7253884c2e4 314 else {
Hubertus 5:e7253884c2e4 315 myservo = 0.0;
Hubertus 5:e7253884c2e4 316 wait(0.01);
Hubertus 5:e7253884c2e4 317 }
Hubertus 5:e7253884c2e4 318
Hubertus 5:e7253884c2e4 319
Hubertus 0:df553b18547d 320 }
Hubertus 0:df553b18547d 321 }