Rick Poppe / Mbed 2 deprecated EMG_filter

working version but stripped of most unnecessary code like print statements

Dependencies:   HIDScope MODSERIAL biquadFilter mbed FastPWM QEI

main.cpp@43:6d6c643d3e6d, 2016-10-28 (annotated)

Committer:
RiP
Date:
Fri Oct 28 10:35:58 2016 +0000
Revision:
43:6d6c643d3e6d
Parent:
42:37cd882e7f2b 
calibration toegevoegd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
RiP 21:3aecd735319d 3 #include "BiQuad.h"
RiP 22:f38a15e851d2 4 #include "MODSERIAL.h"
RiP 41:a89907bb3f70 5 #include "QEI.h"
RiP 41:a89907bb3f70 6 #include "FastPWM.h"
vsluiter 0:32bb76391d89 7
RiP 40:1ca50c657cc5 8 // in gebruik: D(0,1,4,5,6,7,8,10,11,12,13) A(0,1,2)
RiP 40:1ca50c657cc5 9
RiP 39:955929e25858 10 MODSERIAL pc(USBTX, USBRX);
RiP 41:a89907bb3f70 11 HIDScope scope(3); // the amount of scopes to send to the pc
RiP 36:344588e69589 12
vsluiter 4:8b298dfada81 13 //Define objects
RiP 24:01b4b51b5dc6 14
RiP 31:9c3b022f1dc3 15 //Define the EMG inputs
RiP 21:3aecd735319d 16 AnalogIn emg1( A0 );
RiP 21:3aecd735319d 17 AnalogIn emg2( A1 );
RiP 21:3aecd735319d 18 AnalogIn emg3( A2 );
tomlankhorst 19:2bf824669684 19
RiP 41:a89907bb3f70 20 //Define motor outputs
RiP 41:a89907bb3f70 21 DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
RiP 41:a89907bb3f70 22 FastPWM motor1(D6); // speed of motor 1
RiP 41:a89907bb3f70 23 FastPWM motor2(D5); //speed of motor 2
RiP 41:a89907bb3f70 24 DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw
RiP 41:a89907bb3f70 25
RiP 41:a89907bb3f70 26 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
RiP 41:a89907bb3f70 27 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
RiP 41:a89907bb3f70 28
RiP 43:6d6c643d3e6d 29 DigitalIn button(PTA4);
RiP 43:6d6c643d3e6d 30
RiP 31:9c3b022f1dc3 31 //Define the Tickers
RiP 39:955929e25858 32 Ticker pos_timer; // the timer which is used to print the position every second
RiP 39:955929e25858 33 Ticker sample_timer; // the timer which is used to decide when a sample needs to be taken
RiP 41:a89907bb3f70 34 Ticker control; // Ticker for processing encoder input
vsluiter 2:e314bb3b2d99 35
RiP 36:344588e69589 36 //Initialize all variables
RiP 39:955929e25858 37 volatile bool sampletimer = false; // a variable which is changed when a sample needs to be taken
RiP 41:a89907bb3f70 38 volatile bool controller_go=false;
RiP 43:6d6c643d3e6d 39 bool button_pressed=false;
RiP 25:1a71424b05ff 40
RiP 39:955929e25858 41 double threshold = 0.04; // the threshold which the emg signals need to surpass to do something
RiP 40:1ca50c657cc5 42 double samplefreq=0.002; // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
RiP 39:955929e25858 43 double emg02; // the first emg signal
RiP 39:955929e25858 44 double emg12; // the second emg signal
RiP 39:955929e25858 45 double emg22; // the third emg signal
RiP 39:955929e25858 46 double ref_x=0.0000; // the x reference position
RiP 39:955929e25858 47 double ref_y=0.0000; // the y reference position
RiP 39:955929e25858 48 double old_ref_x; // the old x reference
RiP 39:955929e25858 49 double old_ref_y; // the old y reference
RiP 39:955929e25858 50 double speed=0.00002; // the variable with which a speed is reached of 1cm/s
RiP 39:955929e25858 51 double theta=0.0; // angle of the arm
RiP 39:955929e25858 52 double radius=0.0; // radius of the arm
RiP 36:344588e69589 53
RiP 43:6d6c643d3e6d 54 double minRadius=0.3; // minimum radius of arm
RiP 39:955929e25858 55 const double maxRadius=0.6; // maximum radius of arm
RiP 43:6d6c643d3e6d 56 const double min_Radius=0.3;
RiP 39:955929e25858 57 const double minAngle=-1.25; // minimum angle for limiting controller
RiP 40:1ca50c657cc5 58 const double min_y=-0.28; // minimum height which the spatula can reach
RiP 39:955929e25858 59 char key; // variable to place the keyboard input
RiP 28:3b1b29193851 60
RiP 41:a89907bb3f70 61 double m1_pwm=0; //variable for PWM control motor 1
RiP 41:a89907bb3f70 62 double m2_pwm=0; //variable for PWM control motor 2
RiP 41:a89907bb3f70 63
RiP 41:a89907bb3f70 64 const double m1_Kp = 9.974, m1_Ki = 16.49, m1_Kd = 1.341, m1_N = 100; // controller constants motor 1
RiP 41:a89907bb3f70 65 double m1_v1 = 0, m1_v2 = 0; // Memory variables
RiP 41:a89907bb3f70 66 const double m1_Ts = 0.01; // Controller sample time
RiP 41:a89907bb3f70 67
RiP 41:a89907bb3f70 68 const double m2_Kp = 9.974, m2_Ki = 16.49, m2_Kd = 1.341, m2_N = 100; // controller constants motor 2
RiP 41:a89907bb3f70 69 double m2_v1 = 0, m2_v2 = 0; // Memory variables
RiP 41:a89907bb3f70 70 const double m2_Ts = 0.01; // Controller sample time
RiP 41:a89907bb3f70 71
RiP 41:a89907bb3f70 72 const double pi=3.14159265359;
RiP 41:a89907bb3f70 73 const double res = 64/(1/131.25*2*pi); // resolution on gearbox shaft per pulse
RiP 41:a89907bb3f70 74 const double V_max=9.0; // maximum voltage supplied by trafo
RiP 43:6d6c643d3e6d 75 const double pulleyRadius=0.0398/2; // pulley diameter
RiP 41:a89907bb3f70 76
RiP 31:9c3b022f1dc3 77 //Define the needed Biquad chains
RiP 21:3aecd735319d 78 BiQuadChain bqc11;
RiP 21:3aecd735319d 79 BiQuadChain bqc13;
RiP 21:3aecd735319d 80 BiQuadChain bqc21;
RiP 21:3aecd735319d 81 BiQuadChain bqc23;
RiP 21:3aecd735319d 82 BiQuadChain bqc31;
RiP 21:3aecd735319d 83 BiQuadChain bqc33;
RiP 31:9c3b022f1dc3 84
RiP 31:9c3b022f1dc3 85 //Define the BiQuads for the filter of the first emg signal
RiP 31:9c3b022f1dc3 86 //Notch filter
RiP 21:3aecd735319d 87 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 88 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 89 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 31:9c3b022f1dc3 90 //High pass filter
RiP 36:344588e69589 91 //BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
RiP 36:344588e69589 92 BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941);
RiP 36:344588e69589 93 BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450);
RiP 36:344588e69589 94 BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403);
RiP 31:9c3b022f1dc3 95 //Low pass filter
RiP 21:3aecd735319d 96 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 97
RiP 31:9c3b022f1dc3 98 //Define the BiQuads for the filter of the second emg signal
RiP 31:9c3b022f1dc3 99 //Notch filter
RiP 31:9c3b022f1dc3 100 BiQuad bq211 = bq111;
RiP 31:9c3b022f1dc3 101 BiQuad bq212 = bq112;
RiP 31:9c3b022f1dc3 102 BiQuad bq213 = bq113;
RiP 39:955929e25858 103 //High pass filter
RiP 31:9c3b022f1dc3 104 BiQuad bq221 = bq121;
RiP 36:344588e69589 105 BiQuad bq222 = bq122;
RiP 36:344588e69589 106 BiQuad bq223 = bq123;
RiP 39:955929e25858 107 //Low pass filter
RiP 31:9c3b022f1dc3 108 BiQuad bq231 = bq131;
RiP 21:3aecd735319d 109
RiP 31:9c3b022f1dc3 110 //Define the BiQuads for the filter of the third emg signal
RiP 31:9c3b022f1dc3 111 //notch filter
RiP 31:9c3b022f1dc3 112 BiQuad bq311 = bq111;
RiP 31:9c3b022f1dc3 113 BiQuad bq312 = bq112;
RiP 31:9c3b022f1dc3 114 BiQuad bq313 = bq113;
RiP 31:9c3b022f1dc3 115 //High pass filter
RiP 31:9c3b022f1dc3 116 BiQuad bq321 = bq121;
RiP 36:344588e69589 117 BiQuad bq323 = bq122;
RiP 36:344588e69589 118 BiQuad bq322 = bq123;
RiP 31:9c3b022f1dc3 119 //low pass filter
RiP 31:9c3b022f1dc3 120 BiQuad bq331 = bq131;
RiP 22:f38a15e851d2 121
RiP 22:f38a15e851d2 122 void sampleflag()
RiP 22:f38a15e851d2 123 {
mefix 38:0c8a6b0834da 124 if (sampletimer==true) {
RiP 39:955929e25858 125 // this if statement is used to see if the code takes too long before it is called again
RiP 39:955929e25858 126 pc.printf("rate too high error in sampleflag\n\r");
RiP 37:af85a7b57a25 127 }
RiP 36:344588e69589 128 //This sets the go flag for when the function sample needs to be called
RiP 22:f38a15e851d2 129 sampletimer=true;
RiP 22:f38a15e851d2 130 }
RiP 22:f38a15e851d2 131
RiP 41:a89907bb3f70 132 void activate_controller()
RiP 24:01b4b51b5dc6 133 {
RiP 41:a89907bb3f70 134 if (controller_go==true) {
RiP 41:a89907bb3f70 135 // this if statement is used to see if the code takes too long before it is called again
RiP 41:a89907bb3f70 136 pc.printf("rate too high error in activate_controller()\n\r");
RiP 41:a89907bb3f70 137 }
RiP 41:a89907bb3f70 138 controller_go=true; //activate go flag
RiP 24:01b4b51b5dc6 139 }
RiP 22:f38a15e851d2 140
RiP 39:955929e25858 141 void sample()
vsluiter 2:e314bb3b2d99 142 {
RiP 39:955929e25858 143 //This checks if a key is pressed and changes the variable key in the pressed key
RiP 33:fcd4568f1c86 144 if (pc.readable()==1) {
RiP 33:fcd4568f1c86 145 key=pc.getc();
mefix 38:0c8a6b0834da 146 }
RiP 36:344588e69589 147 //Read the emg signals and filter it
RiP 22:f38a15e851d2 148
RiP 36:344588e69589 149 emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 0
RiP 36:344588e69589 150 emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 1
RiP 36:344588e69589 151 emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 2
RiP 29:ac08c1a32c54 152
RiP 39:955929e25858 153 //remember what the reference was
RiP 37:af85a7b57a25 154 old_ref_x=ref_x;
RiP 37:af85a7b57a25 155 old_ref_y=ref_y;
RiP 39:955929e25858 156 //look if the emg signals go over the threshold and change the reference accordingly
RiP 33:fcd4568f1c86 157 if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
RiP 37:af85a7b57a25 158 ref_x=ref_x-speed;
RiP 37:af85a7b57a25 159 ref_y=ref_y-speed;
RiP 31:9c3b022f1dc3 160
RiP 33:fcd4568f1c86 161 } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
RiP 37:af85a7b57a25 162 ref_x=ref_x-speed;
RiP 23:54d28f9eef53 163
RiP 33:fcd4568f1c86 164 } else if (emg02>threshold&&emg22>threshold || key == 's') {
RiP 37:af85a7b57a25 165 ref_y=ref_y-speed;
RiP 31:9c3b022f1dc3 166
RiP 33:fcd4568f1c86 167 } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
RiP 31:9c3b022f1dc3 168 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 169 ref_y=ref_y+speed;
RiP 31:9c3b022f1dc3 170
RiP 33:fcd4568f1c86 171 } else if (emg12>threshold || key == 'q' ) {
RiP 31:9c3b022f1dc3 172 ref_x=ref_x+speed;
RiP 40:1ca50c657cc5 173
RiP 33:fcd4568f1c86 174 } else if (emg22>threshold || key == 'w') {
RiP 31:9c3b022f1dc3 175 ref_y=ref_y+speed;
RiP 37:af85a7b57a25 176 }
RiP 37:af85a7b57a25 177
RiP 39:955929e25858 178 // convert the x and y reference to the theta and radius reference
RiP 43:6d6c643d3e6d 179 theta=atan(ref_y/(ref_x+min_Radius));
RiP 43:6d6c643d3e6d 180 radius=sqrt(pow(ref_x+min_Radius,2)+pow(ref_y,2));
mefix 38:0c8a6b0834da 181
RiP 39:955929e25858 182 //look if the new reference is outside the possible range and revert back to the old reference if it is outside the range
mefix 38:0c8a6b0834da 183 if (theta < minAngle) {
RiP 37:af85a7b57a25 184 ref_x=old_ref_x;
RiP 37:af85a7b57a25 185 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 186
RiP 37:af85a7b57a25 187 } else if (radius < minRadius) {
RiP 37:af85a7b57a25 188 ref_x=old_ref_x;
RiP 37:af85a7b57a25 189 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 190
mefix 38:0c8a6b0834da 191 } else if ( radius > maxRadius) {
RiP 37:af85a7b57a25 192 ref_x=old_ref_x;
RiP 37:af85a7b57a25 193 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 194 } else if (ref_y<min_y) {
RiP 40:1ca50c657cc5 195 ref_x=old_ref_x;
RiP 40:1ca50c657cc5 196 ref_y=old_ref_y;
RiP 22:f38a15e851d2 197 }
RiP 43:6d6c643d3e6d 198 theta=atan(ref_y/(ref_x+min_Radius));
RiP 43:6d6c643d3e6d 199 radius=sqrt(pow(ref_x+min_Radius,2)+pow(ref_y,2));
RiP 41:a89907bb3f70 200 }
RiP 41:a89907bb3f70 201
RiP 41:a89907bb3f70 202 double PID( double err, const double Kp, const double Ki, const double Kd,
RiP 41:a89907bb3f70 203 const double Ts, const double N, double &v1, double &v2 ) //discrete PIDF filter
RiP 41:a89907bb3f70 204 {
RiP 41:a89907bb3f70 205 const double a1 =-4/(N*Ts+2),
RiP 41:a89907bb3f70 206 a2=-(N*Ts-2)/(N*Ts+2),
RiP 41:a89907bb3f70 207 b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
RiP 41:a89907bb3f70 208 b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
RiP 41:a89907bb3f70 209 b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
RiP 41:a89907bb3f70 210
RiP 41:a89907bb3f70 211 double v=err-a1*v1-a2*v2;
RiP 41:a89907bb3f70 212 double u=b0*v+b1*v1+b2*v2;
RiP 41:a89907bb3f70 213 v2=v1;
RiP 41:a89907bb3f70 214 v1=v;
RiP 41:a89907bb3f70 215 return u;
RiP 41:a89907bb3f70 216 }
RiP 41:a89907bb3f70 217
RiP 41:a89907bb3f70 218 void controller() //function for executing controller action
RiP 41:a89907bb3f70 219 {
RiP 41:a89907bb3f70 220
RiP 41:a89907bb3f70 221 //converting radius and theta to gearbox angle
RiP 41:a89907bb3f70 222 double ref_angle1=16*theta;
RiP 43:6d6c643d3e6d 223 double ref_angle2=(-radius+min_Radius)/pulleyRadius;
RiP 41:a89907bb3f70 224
RiP 41:a89907bb3f70 225 double angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
RiP 41:a89907bb3f70 226 double angle2 = Encoder2.getPulses()/res; //get number of pulses
RiP 41:a89907bb3f70 227 m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
RiP 41:a89907bb3f70 228 //divide by voltage to get pwm duty cycle percentage)
RiP 41:a89907bb3f70 229 m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
RiP 41:a89907bb3f70 230
RiP 41:a89907bb3f70 231 //limit pwm value and change motor direction when pwm becomes either negative or positive
RiP 41:a89907bb3f70 232 if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
RiP 41:a89907bb3f70 233 motor1dir=0;
RiP 41:a89907bb3f70 234 motor1.write(m1_pwm);
RiP 41:a89907bb3f70 235 } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
RiP 41:a89907bb3f70 236 motor1dir=1;
RiP 41:a89907bb3f70 237 motor1.write(-m1_pwm);
RiP 41:a89907bb3f70 238 }
RiP 41:a89907bb3f70 239
RiP 41:a89907bb3f70 240 if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
RiP 41:a89907bb3f70 241 motor2dir=0;
RiP 41:a89907bb3f70 242 motor2.write(m2_pwm);
RiP 41:a89907bb3f70 243 } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
RiP 41:a89907bb3f70 244 motor2dir=1;
RiP 41:a89907bb3f70 245 motor2.write(-m2_pwm);
RiP 41:a89907bb3f70 246 }
RiP 41:a89907bb3f70 247
RiP 41:a89907bb3f70 248 //hidsopce to check what the code does exactly
RiP 41:a89907bb3f70 249 scope.set(0,ref_angle2-angle2); //error
RiP 41:a89907bb3f70 250 scope.set(1,angle2);
RiP 41:a89907bb3f70 251 scope.set(2,m2_pwm);
RiP 41:a89907bb3f70 252 scope.send();
vsluiter 2:e314bb3b2d99 253 }
vsluiter 0:32bb76391d89 254
RiP 27:1ff7fa636f1c 255 void my_pos()
RiP 27:1ff7fa636f1c 256 {
RiP 36:344588e69589 257 //This function is attached to a ticker so that the reference position is printed every second.
RiP 37:af85a7b57a25 258 pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
RiP 27:1ff7fa636f1c 259
RiP 27:1ff7fa636f1c 260 }
RiP 26:91d48c0b722d 261
vsluiter 0:32bb76391d89 262 int main()
RiP 21:3aecd735319d 263 {
RiP 29:ac08c1a32c54 264 pc.printf("RESET\n\r");
RiP 22:f38a15e851d2 265 pc.baud(115200);
RiP 22:f38a15e851d2 266
RiP 39:955929e25858 267 //Attach the Biquads to the Biquad chains
RiP 36:344588e69589 268 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
RiP 21:3aecd735319d 269 bqc13.add( &bq131);
RiP 36:344588e69589 270 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
RiP 21:3aecd735319d 271 bqc23.add( &bq231);
RiP 36:344588e69589 272 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
RiP 21:3aecd735319d 273 bqc33.add( &bq331);
RiP 36:344588e69589 274
RiP 41:a89907bb3f70 275 motor1.period(0.02f); //period of pwm signal for motor 1
RiP 41:a89907bb3f70 276 motor2.period(0.02f); // period of pwm signal for motor 2
RiP 41:a89907bb3f70 277 motor1dir=0; // setting direction to ccw
RiP 41:a89907bb3f70 278 motor2dir=0; // setting direction to ccw
RiP 41:a89907bb3f70 279
RiP 36:344588e69589 280 //Attach the 'sample' function to the timer 'sample_timer'.
RiP 36:344588e69589 281 //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
RiP 23:54d28f9eef53 282 sample_timer.attach(&sampleflag, samplefreq);
RiP 41:a89907bb3f70 283
RiP 36:344588e69589 284 //Attach the function my_pos to the timer pos_timer.
RiP 36:344588e69589 285 //This ensures that the position is printed every second.
RiP 31:9c3b022f1dc3 286 pos_timer.attach(&my_pos, 1);
RiP 41:a89907bb3f70 287 control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
RiP 41:a89907bb3f70 288
tomlankhorst 15:0da764eea774 289
RiP 22:f38a15e851d2 290 while(1) {
RiP 39:955929e25858 291 //Only take a sample when the go flag is true.
RiP 43:6d6c643d3e6d 292 if (button==0) {
RiP 43:6d6c643d3e6d 293 button_pressed=true;
RiP 43:6d6c643d3e6d 294 minRadius=-50.0;
RiP 43:6d6c643d3e6d 295 } else {
RiP 43:6d6c643d3e6d 296 if (button_pressed==true) {
RiP 43:6d6c643d3e6d 297 ref_x=0.0;
RiP 43:6d6c643d3e6d 298 ref_y=0.0;
RiP 43:6d6c643d3e6d 299 button_pressed=false;
RiP 43:6d6c643d3e6d 300 }
RiP 43:6d6c643d3e6d 301 minRadius=0.3;
RiP 43:6d6c643d3e6d 302 }
RiP 43:6d6c643d3e6d 303
RiP 22:f38a15e851d2 304 if (sampletimer==true) {
RiP 39:955929e25858 305 sample();
RiP 39:955929e25858 306 sampletimer = false; //change sampletimer to false if sample() is finished
RiP 25:1a71424b05ff 307 }
RiP 43:6d6c643d3e6d 308
RiP 41:a89907bb3f70 309 if(controller_go) { // go flag
RiP 41:a89907bb3f70 310 controller();
RiP 41:a89907bb3f70 311 controller_go=false;
RiP 41:a89907bb3f70 312 }
RiP 22:f38a15e851d2 313 }
vsluiter 0:32bb76391d89 314 }