Margreeth de Breij / Mbed 2 deprecated frdm_MotorEMG

Motor programma met EMG

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_3 by Margreeth de Breij

main.cpp@27:3392f03bfada, 2015-10-08 (annotated)

Committer:
Margreeth95
Date:
Thu Oct 08 18:10:26 2015 +0000
Revision:
27:3392f03bfada
Parent:
26:cd1db85359aa
Child:
28:a40884792e0a
EMG signalen toegevoegd, nog niet getest

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rvs94 25:ae908de29943 1 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 2 // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
Rvs94 25:ae908de29943 3 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 4 // Libraries
Rvs94 25:ae908de29943 5 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 6 #include "mbed.h"
Margreeth95 0:284ed397e046 7 #include "MODSERIAL.h"
Margreeth95 0:284ed397e046 8 #include "HIDScope.h"
Margreeth95 0:284ed397e046 9 #include "QEI.h"
Rvs94 12:69ab81cf5b7d 10 #include "biquadFilter.h"
Rvs94 25:ae908de29943 11
Rvs94 25:ae908de29943 12 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 13 // Constanten/Inputs/Outputs
Rvs94 25:ae908de29943 14 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 15 MODSERIAL pc(USBTX, USBRX); // To/From PC
Rvs94 25:ae908de29943 16 QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2
Rvs94 25:ae908de29943 17 QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1
Rvs94 25:ae908de29943 18 HIDScope scope(4); // Scope, 4 channels
Rvs94 20:f5091e29cd26 19
Rvs94 25:ae908de29943 20 // LEDs
Rvs94 25:ae908de29943 21 DigitalOut LedR(LED_RED);
Rvs94 25:ae908de29943 22 DigitalOut LedG(LED_GREEN);
Rvs94 25:ae908de29943 23 DigitalOut LedB(LED_BLUE);
Rvs94 20:f5091e29cd26 24
Rvs94 25:ae908de29943 25 // Motor
Rvs94 25:ae908de29943 26 DigitalOut motor1direction(D7); // Motor 1, Direction & Speed
Rvs94 25:ae908de29943 27 PwmOut motor1speed(D6);
Rvs94 25:ae908de29943 28 DigitalOut motor2direction(D4); // Motor 2, Direction & Speed
Rvs94 25:ae908de29943 29 PwmOut motor2speed(D5);
Rvs94 7:67b50d4fb03c 30
Margreeth95 27:3392f03bfada 31 //EMG
Margreeth95 27:3392f03bfada 32 AnalogIn EMG_left(A0); //Analog input
Margreeth95 27:3392f03bfada 33 AnalogIn EMG_right(A1);
Rvs94 25:ae908de29943 34 // Tickers
Rvs94 25:ae908de29943 35 Ticker ScopeTime;
Rvs94 25:ae908de29943 36 Ticker myControllerTicker2;
Rvs94 25:ae908de29943 37 Ticker myControllerTicker1;
Margreeth95 27:3392f03bfada 38 Ticker SampleEMG;
Rvs94 25:ae908de29943 39
Rvs94 25:ae908de29943 40 // Constants
Rvs94 25:ae908de29943 41 double reference2, reference1;
Rvs94 25:ae908de29943 42 double position2 = 0, position1 = 0;
Rvs94 25:ae908de29943 43 double m2_ref = 0, m1_ref = 0;
Rvs94 25:ae908de29943 44 int count = 0;
Rvs94 25:ae908de29943 45 double Grens2 = 90, Grens1 = 90;
Rvs94 25:ae908de29943 46 double Stapgrootte = 5;
Margreeth95 27:3392f03bfada 47
Margreeth95 27:3392f03bfada 48 double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
Margreeth95 27:3392f03bfada 49 double EMG_L_fh=0;
Margreeth95 27:3392f03bfada 50 double EMG_left_value;
Margreeth95 27:3392f03bfada 51 double EMG_left_f1;
Margreeth95 27:3392f03bfada 52 double EMG_left_f2;
Margreeth95 27:3392f03bfada 53 double EMG_left_abs;
Margreeth95 27:3392f03bfada 54 double EMG_right_value;
Margreeth95 27:3392f03bfada 55 double EMG_right_f1;
Margreeth95 27:3392f03bfada 56 double EMG_right_f2;
Margreeth95 27:3392f03bfada 57 double EMG_right_abs;
Margreeth95 27:3392f03bfada 58 double Threshold1 = 0.08;
Margreeth95 27:3392f03bfada 59 double Threshold2 = 0.06;
Rvs94 2:099da0fc31b6 60
Rvs94 25:ae908de29943 61 //Sample time (motor-step)
Rvs94 25:ae908de29943 62 const double m2_Ts = 0.01, m1_Ts = 0.01;
Rvs94 20:f5091e29cd26 63
Rvs94 25:ae908de29943 64 //Controller gain Motor 2 & 1
Margreeth95 26:cd1db85359aa 65 const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20;
Margreeth95 26:cd1db85359aa 66 const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20;
Rvs94 25:ae908de29943 67 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 25:ae908de29943 68 double m1_err_int = 0, m1_prev_err = 0;
Rvs94 20:f5091e29cd26 69
Rvs94 25:ae908de29943 70 //Derivative filter coeffs Motor 2 & 1
Margreeth95 26:cd1db85359aa 71 const double BiGain2 = 0.012, BiGain1 = 0.016955;
Rvs94 25:ae908de29943 72 const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
Rvs94 25:ae908de29943 73 const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
Margreeth95 27:3392f03bfada 74
Margreeth95 27:3392f03bfada 75 //Filter coeffs for EMG
Margreeth95 27:3392f03bfada 76 const double BiGainEMG_Lh = 0.723601, BiGainEMG_Ll=0.959332;
Margreeth95 27:3392f03bfada 77 const double EMGh_a1 = -1.74355513773*BiGainEMG_Lh, EMGh_a2 = 0.80079826172*BiGainEMG_Lh, EMGh_b0 = 1.0*BiGainEMG_Lh, EMGh_b1 = 1.99999965990*BiGainEMG_Lh, EMGh_b2 = 1.0*BiGainEMG_Lh; //coefficients for high-pass filter
Margreeth95 27:3392f03bfada 78 const double EMGl_a1 = 1.91721405106*BiGainEMG_Ll, EMGl_a2 = 0.92055427516*BiGainEMG_Ll, EMGl_b0 = 1.0*BiGainEMG_Ll, EMGl_b1 = 1.99999993582*BiGainEMG_Ll, EMGl_b2 = 1.0*BiGainEMG_Ll; // coefficients for low-pass filter
Rvs94 20:f5091e29cd26 79
Rvs94 20:f5091e29cd26 80 // Filter variables
Rvs94 25:ae908de29943 81 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 25:ae908de29943 82 double m1_f_v1 = 0, m1_f_v2 = 0;
Margreeth95 27:3392f03bfada 83
Margreeth95 27:3392f03bfada 84 // Creating the filters
Margreeth95 27:3392f03bfada 85 biquadFilter EMG_highpass (EMGh_a1, EMGh_a2, EMGh_b0, EMGh_b1, EMGh_b2); // creates the high pass filter for EMG
Margreeth95 27:3392f03bfada 86 biquadFilter EMG_lowpass (EMGl_a1, EMGl_a2, EMGl_b0, EMGl_b1, EMGl_b2); // creates the low pass filter for EMG
Rvs94 20:f5091e29cd26 87
Rvs94 25:ae908de29943 88 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 89 // General Functions
Rvs94 25:ae908de29943 90 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 20:f5091e29cd26 91
Rvs94 20:f5091e29cd26 92 //HIDScope
Rvs94 25:ae908de29943 93 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Rvs94 25:ae908de29943 94 {
Rvs94 25:ae908de29943 95 scope.set(0, reference2 - position2);
Rvs94 25:ae908de29943 96 scope.set(1, position2);
Rvs94 25:ae908de29943 97 scope.set(2, reference1 - position1);
Rvs94 25:ae908de29943 98 scope.set(3, position1);
Rvs94 25:ae908de29943 99 scope.send();
Rvs94 1:48aba8d5610a 100
Rvs94 25:ae908de29943 101 }
Rvs94 12:69ab81cf5b7d 102
Rvs94 12:69ab81cf5b7d 103 // Biquad filter
Rvs94 25:ae908de29943 104 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
Rvs94 25:ae908de29943 105 {
Rvs94 25:ae908de29943 106 double v = u - a1*v1 - a2*v2;
Rvs94 25:ae908de29943 107 double y = b0*v + b1*v1 + b2*v2;
Rvs94 25:ae908de29943 108 v2 = v1; v1 = v;
Rvs94 25:ae908de29943 109 return y;
Rvs94 25:ae908de29943 110 }
Rvs94 12:69ab81cf5b7d 111
Rvs94 12:69ab81cf5b7d 112
Rvs94 12:69ab81cf5b7d 113 // Reusable PID controller
Rvs94 25:ae908de29943 114 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
Rvs94 25:ae908de29943 115 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 25:ae908de29943 116 {
Rvs94 12:69ab81cf5b7d 117 // Derivative
Rvs94 25:ae908de29943 118 double e_der = (e-e_prev)/Ts;
Rvs94 25:ae908de29943 119 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 25:ae908de29943 120 e_prev = e;
Rvs94 12:69ab81cf5b7d 121 // Integral
Rvs94 25:ae908de29943 122 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 123 // PID
Rvs94 25:ae908de29943 124 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 25:ae908de29943 125 }
Margreeth95 26:cd1db85359aa 126
Margreeth95 26:cd1db85359aa 127 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 26:cd1db85359aa 128 //EMG functions
Margreeth95 26:cd1db85359aa 129 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 26:cd1db85359aa 130
Margreeth95 27:3392f03bfada 131 // EMG filtering function
Margreeth95 27:3392f03bfada 132 void EMGfilter() // Both EMG signals are filtered in one function and with the same filters
Margreeth95 27:3392f03bfada 133 {
Margreeth95 27:3392f03bfada 134 EMG_left_value = EMG_left.read();
Margreeth95 27:3392f03bfada 135 EMG_left_f1 = EMG_highpass.step(EMG_left_value);
Margreeth95 27:3392f03bfada 136 EMG_left_f2 = EMG_lowpass.step(EMG_left_f1);
Margreeth95 27:3392f03bfada 137 EMG_left_abs = fabs(EMG_left_f2);
Margreeth95 27:3392f03bfada 138
Margreeth95 27:3392f03bfada 139 EMG_right_value = EMG_right.read();
Margreeth95 27:3392f03bfada 140 EMG_right_f1 = EMG_highpass.step(EMG_right_value);
Margreeth95 27:3392f03bfada 141 EMG_right_f1 = EMG_lowpass.step(EMG_right_f1);
Margreeth95 27:3392f03bfada 142 EMG_right_abs = fabs(EMG_right_f2);
Margreeth95 27:3392f03bfada 143 }
Margreeth95 26:cd1db85359aa 144
Rvs94 25:ae908de29943 145 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 146 // Motor control functions
Rvs94 25:ae908de29943 147 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 148
Margreeth95 18:6f71bb91b8bd 149 // Motor2 control
Rvs94 25:ae908de29943 150 void motor2_Controller()
Rvs94 9:774fc3c6a39e 151 {
Rvs94 25:ae908de29943 152 // Setpoint motor 2
Rvs94 25:ae908de29943 153 reference2 = m2_ref; // Reference in degrees
Rvs94 25:ae908de29943 154 position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 155 // Speed control
Rvs94 25:ae908de29943 156 double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,
Rvs94 25:ae908de29943 157 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 25:ae908de29943 158 double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); // Filter of motorspeed input
Rvs94 25:ae908de29943 159 motor2speed = abs(m2_P2);
Rvs94 25:ae908de29943 160 // Direction control
Rvs94 25:ae908de29943 161 if(m2_P2 > 0)
Rvs94 25:ae908de29943 162 {
Rvs94 25:ae908de29943 163 motor2direction = 0;
Rvs94 25:ae908de29943 164 }
Rvs94 25:ae908de29943 165 else
Rvs94 25:ae908de29943 166 {
Rvs94 25:ae908de29943 167 motor2direction = 1;
Rvs94 25:ae908de29943 168 }
Rvs94 25:ae908de29943 169 }
Rvs94 25:ae908de29943 170
Rvs94 25:ae908de29943 171 // Motor1 control
Rvs94 25:ae908de29943 172 void motor1_Controller()
Rvs94 25:ae908de29943 173 {
Rvs94 25:ae908de29943 174 // Setpoint Motor 1
Rvs94 25:ae908de29943 175 reference1 = m1_ref; // Reference in degrees
Rvs94 25:ae908de29943 176 position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 177 // Speed control
Rvs94 25:ae908de29943 178 double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2,
Rvs94 25:ae908de29943 179 m1_f_b0, m1_f_b1, m1_f_b2);
Rvs94 25:ae908de29943 180 double m1_P2 = biquad(m1_P1, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
Rvs94 25:ae908de29943 181 motor1speed = abs(m1_P2);
Rvs94 25:ae908de29943 182 // Direction control
Rvs94 25:ae908de29943 183 if(m1_P2 > 0)
Rvs94 25:ae908de29943 184 {
Rvs94 25:ae908de29943 185 motor1direction = 1;
Rvs94 25:ae908de29943 186 }
Rvs94 25:ae908de29943 187 else
Rvs94 25:ae908de29943 188 {
Rvs94 25:ae908de29943 189 motor1direction = 0;
Rvs94 25:ae908de29943 190 }
Rvs94 9:774fc3c6a39e 191 }
Rvs94 3:687729d7996e 192
Rvs94 25:ae908de29943 193 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 194 // Main function
Rvs94 25:ae908de29943 195 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 196 int main()
Rvs94 25:ae908de29943 197 {
Rvs94 25:ae908de29943 198 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 199 // Initalizing
Rvs94 25:ae908de29943 200 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 201 //LEDs OFF
Rvs94 25:ae908de29943 202 LedR = LedB = LedG = 1;
Rvs94 9:774fc3c6a39e 203
Rvs94 25:ae908de29943 204 //PC connection & check
Rvs94 25:ae908de29943 205 pc.baud(115200);
Rvs94 25:ae908de29943 206 pc.printf("Tot aan loop werkt\n");
Rvs94 25:ae908de29943 207
Rvs94 25:ae908de29943 208 // Tickers
Rvs94 25:ae908de29943 209 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
Rvs94 25:ae908de29943 210 myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
Rvs94 25:ae908de29943 211 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
Margreeth95 27:3392f03bfada 212 SampleEMG.attach(&EMGfilter, 0.01f); // 100Hz, EMG signalen
Rvs94 25:ae908de29943 213
Rvs94 25:ae908de29943 214 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 215 // Control Program
Rvs94 25:ae908de29943 216 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 9:774fc3c6a39e 217 while(true)
Rvs94 20:f5091e29cd26 218 {
Rvs94 20:f5091e29cd26 219 char c = pc.getc();
Rvs94 25:ae908de29943 220 // 1 Program UP
Margreeth95 27:3392f03bfada 221 if ((EMG_right_abs >= Threshold1) && (EMG_left_abs >= Threshold1)) //if(c == 'e')
Rvs94 20:f5091e29cd26 222 {
Rvs94 20:f5091e29cd26 223 count = count + 1;
Rvs94 20:f5091e29cd26 224 if(count > 2)
Rvs94 20:f5091e29cd26 225 {
Rvs94 20:f5091e29cd26 226 count = 2;
Rvs94 20:f5091e29cd26 227 }
Rvs94 20:f5091e29cd26 228
Rvs94 20:f5091e29cd26 229 }
Rvs94 25:ae908de29943 230 // 1 Program DOWN
Margreeth95 26:cd1db85359aa 231 if(c == 'd') // Hoe gaat dit aangestuurd worden?
Rvs94 20:f5091e29cd26 232 {
Rvs94 20:f5091e29cd26 233 count = count - 1;
Rvs94 20:f5091e29cd26 234 if(count < 0)
Rvs94 20:f5091e29cd26 235 {
Rvs94 20:f5091e29cd26 236 count = 0;
Rvs94 20:f5091e29cd26 237 }
Rvs94 25:ae908de29943 238 }
Rvs94 25:ae908de29943 239 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
Rvs94 25:ae908de29943 240 if(count == 0)
Rvs94 20:f5091e29cd26 241 {
Rvs94 20:f5091e29cd26 242
Rvs94 20:f5091e29cd26 243 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 244 LedG = 0;
Margreeth95 27:3392f03bfada 245 if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) //if(c == 'r')
Margreeth95 19:9417d2011e8b 246 {
Rvs94 24:d0af4b2be295 247 m2_ref = m2_ref + Stapgrootte;
Rvs94 25:ae908de29943 248 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 249 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 250 {
Rvs94 24:d0af4b2be295 251 m2_ref = Grens2;
Rvs94 25:ae908de29943 252 m1_ref = -1*Grens1;
Margreeth95 19:9417d2011e8b 253 }
Margreeth95 19:9417d2011e8b 254 }
Margreeth95 27:3392f03bfada 255 if ((EMG_left_abs > Threshold1) && (EMG_right_abs < Threshold1)) //if(c == 'f')
Margreeth95 19:9417d2011e8b 256 {
Rvs94 24:d0af4b2be295 257 m2_ref = m2_ref - Stapgrootte;
Rvs94 25:ae908de29943 258 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 259 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 260 {
Rvs94 24:d0af4b2be295 261 m2_ref = -1*Grens2;
Rvs94 25:ae908de29943 262 m1_ref = Grens1;
Margreeth95 19:9417d2011e8b 263 }
Margreeth95 19:9417d2011e8b 264 }
Rvs94 20:f5091e29cd26 265 }
Rvs94 25:ae908de29943 266 // PROGRAM 1: Motor 1 control, Red LED
Rvs94 25:ae908de29943 267 if(count == 1)
Rvs94 20:f5091e29cd26 268 {
Rvs94 20:f5091e29cd26 269 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 270 LedR = 0;
Margreeth95 27:3392f03bfada 271 if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) //if(c == 't')
Rvs94 24:d0af4b2be295 272 {
Rvs94 24:d0af4b2be295 273 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 274 if (m1_ref > Grens1)
Rvs94 24:d0af4b2be295 275 {
Rvs94 24:d0af4b2be295 276 m1_ref = Grens1;
Rvs94 24:d0af4b2be295 277 }
Rvs94 24:d0af4b2be295 278 }
Margreeth95 27:3392f03bfada 279 if ((EMG_left_abs > Threshold1) && (EMG_right_abs < Threshold1)) //if(c == 'g')
Rvs94 24:d0af4b2be295 280 {
Rvs94 24:d0af4b2be295 281 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 282 if (m1_ref < -1*Grens1)
Rvs94 24:d0af4b2be295 283 {
Rvs94 24:d0af4b2be295 284 m1_ref = -1*Grens1;
Rvs94 24:d0af4b2be295 285 }
Rvs94 24:d0af4b2be295 286 }
Rvs94 20:f5091e29cd26 287 }
Rvs94 25:ae908de29943 288 // PROGRAM 2: Firing mechanism & Reset, Blue LED
Rvs94 25:ae908de29943 289 if(count == 2)
Rvs94 20:f5091e29cd26 290 {
Rvs94 20:f5091e29cd26 291
Rvs94 20:f5091e29cd26 292 LedR = LedG = 1;
Rvs94 24:d0af4b2be295 293 LedB = 0;
Rvs94 25:ae908de29943 294 //VUUUUR!! (To Do)
Rvs94 24:d0af4b2be295 295 wait(1);
Rvs94 24:d0af4b2be295 296 m2_ref = 0;
Margreeth95 26:cd1db85359aa 297 m1_ref = 0;
Margreeth95 26:cd1db85359aa 298 count = 0;
Rvs94 20:f5091e29cd26 299 }
Margreeth95 0:284ed397e046 300 }
Rvs94 9:774fc3c6a39e 301
Margreeth95 0:284ed397e046 302 }