Alle drie de signalen gefilterd en binair gemaakt
Dependencies: mbed HIDScope biquadFilter
Diff: main.cpp
- Revision:
- 23:a6f18aee31cd
- Parent:
- 22:611667172ac3
- Child:
- 25:cfd422c3cf4d
--- a/main.cpp Wed Oct 30 08:28:29 2019 +0000 +++ b/main.cpp Wed Oct 30 12:11:18 2019 +0000 @@ -16,115 +16,190 @@ HIDScope scope( 5 ); DigitalOut led(LED1); -const int leng_filt = 20; +const int leng_filt = 60; +const int box_length = 100; +const int box_lengthC = 200; +const int box_checkC = 150; +const float grenswaardeA = 0.01; +const float grenswaardeB = 0.02; +const float grenswaardeC = 0.28; + +float Ay1; +float Ay2; float A_array[leng_filt] = {0}; +float A_ar[leng_filt] = {0}; +float A_ar2[leng_filt] = {0}; +float A_ar3[box_length] = {0}; + +float By1; +float By2; float B_array[leng_filt] = {0}; -float Asum_ar[leng_filt] = {0}; -float Bsum_ar[leng_filt] = {0}; -float Asum_ar2[leng_filt] = {0}; -float Bsum_ar2[leng_filt] = {0}; +float B_ar[leng_filt] = {0}; +float B_ar2[leng_filt] = {0}; +float B_ar3[box_length] = {0}; + +float Cy1; +float Cy2; +float C_array[leng_filt] = {0}; +float C_ar[leng_filt] = {0}; +float C_ar2[leng_filt] = {0}; +float C_ar3[box_length] = {0}; + float result = 0; float Asum = 0; -float Bsum = 0; - +const int Fs = 2000; //Sample Frequency +const double b0 = 0.206572; +const double b1 = 0.413144; +const double b2 = 0.206572; +const double a0 = 1.000000; +const double a1 = -0.369527; +const double a2 = 0.195816; void sample() { - + BiQuad lowpass(b0,b1, b2, a0, a1, a2); + // Signaal 1 (A) float A = emg0.read(); + float Amean = 0; + float Ay2 = 0; for (int j=leng_filt-1; j>=1; j--) - { - A_array[j] = A_array[j-1]; - } + { A_ar[j] = A_ar[j-1]; } + + A_ar[0] = A; + + for(int i=0; i<=leng_filt-1; i++) + { Amean += A_ar[i]*1/leng_filt; } + + Ay1 = A - Amean; + Ay1 = fabs(Ay1); + Ay1 = lowpass.step(Ay1); // First signal, after Butterworth + + for (int j=leng_filt-1; j>=1; j--) + { A_ar2[j] = A_ar2[j-1]; } + + A_ar2[0] = Ay1; + + for(int i=0; i<=leng_filt-1; i++) + { Ay2 += A_ar2[i]*1/leng_filt; } + + float Ay3; + if(Ay2>grenswaardeA) + { Ay3 = 1; } + + if(Ay2<=grenswaardeA) + { Ay3 = 0; } + + for (int j=box_length-1; j>=1; j--) + { A_ar3[j] = A_ar3[j-1]; } - A_array[0] = A; - Asum = 0; + A_ar3[0] = Ay3; + int boxcheckA = 0; + + for (int j=0; j<=box_length-1; j++) + { if(A_ar3[j] == 1) + { boxcheckA = 1;} } + + // Signaal 2 (B) + + float B = emg0.read(); + float Bmean = 0; + float By2 = 0; + + for (int j=leng_filt-1; j>=1; j--) + { B_ar[j] = B_ar[j-1]; } + + B_ar[0] = B; + + for(int i=0; i<=leng_filt-1; i++) + { Bmean += B_ar[i]*1/leng_filt; } + + By1 = B - Bmean; + By1 = fabs(By1); + By1 = lowpass.step(By1); // First signal, after Butterworth + + for (int j=leng_filt-1; j>=1; j--) + { B_ar2[j] = B_ar2[j-1]; } + + B_ar2[0] = By1; for(int i=0; i<=leng_filt-1; i++) - { - Asum += A_array[i]*1/leng_filt; - } + { By2 += B_ar2[i]*1/leng_filt; } + + float By3; + if(By2>grenswaardeB) + { By3 = 1; } - // Vanaf hier begint het butterworth laagdoorlaatfilter + if(By2<=grenswaardeB) + { By3 = 0; } + + for (int j=box_length-1; j>=1; j--) + { B_ar3[j] = B_ar3[j-1]; } - const int Fs = 2000; //Sample Frequency - const double b0 = 0.292893; - const double b1 = 0.585786; - const double b2 = 0.292893; - const double a0 = 1.000000; - const double a1 = -0; - const double a2 = 0.171573; + B_ar3[0] = By3; + int boxcheckB = 0; + + for (int j=0; j<=box_length-1; j++) + { if(B_ar3[j] == 1) + { boxcheckB = 1;} } - BiQuad lowpass(b0,b1, b2, a0, a1, a2); + // Signaal 3 (C) + + float C = emg0.read(); + float Cmean = 0; + float Cy2 = 0; + + for (int j=leng_filt-1; j>=1; j--) + { C_ar[j] = C_ar[j-1]; } - // voor bepaald ingangssignaal u1 en output y1 - // Eerst mean dan Buttterworth - - double u1 = Asum; - double y1; - - float Amean = 0; - float Amean2 = 0; - - for (int j=leng_filt-1; j>=1; j--) - { - Asum_ar[j] = Asum_ar[j-1]; - } - - Asum_ar[0] = Asum; + C_ar[0] = C; for(int i=0; i<=leng_filt-1; i++) - { - Amean += Asum_ar[i]*1/leng_filt; - } - - y1 = u1 - Amean; //offset? - y1 = fabs(y1); - y1 = lowpass.step(y1); + { Cmean += C_ar[i]*1/leng_filt; } - // Eerst butterworth dan mean - float y2; - y2 = A - Asum; - y2 = fabs(y2); - y2 = lowpass.step(y2); + Cy1 = C - Cmean; + Cy1 = fabs(Cy1); + Cy1 = lowpass.step(Cy1); // First signal, after Butterworth + + for (int j=leng_filt-1; j>=1; j--) + { C_ar2[j] = C_ar2[j-1]; } - for (int j=leng_filt-1; j>=1; j--) - { - Asum_ar2[j] = Asum_ar2[j-1]; - } - - Asum_ar2[0] = y2; + C_ar2[0] = Cy1; for(int i=0; i<=leng_filt-1; i++) - { - Amean2 += Asum_ar2[i]*1/leng_filt; - } - + { Cy2 += C_ar2[i]*1/leng_filt; } + + float Cy3; + if(Cy2>grenswaardeC) + { Cy3 = 1; } + + if(Cy2<=grenswaardeC) + { Cy3 = 0; } - /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ + for (int j=box_lengthC-1; j>=1; j--) + { C_ar3[j] = C_ar3[j-1]; } + C_ar3[0] = Cy3; + int boxcheckC = 0; + int C_sum = 0; + for (int j=0; j<=box_length-1; j++) + { C_sum += C_ar3[j]; + if(C_sum >= box_checkC) + { boxcheckC = 1;} } + + scope.set(0, emg0.read()); - scope.set(1, Asum); - scope.set(2, y1); - scope.set(3, y2); - scope.set(4, Amean2); + scope.set(1, Cy1); + scope.set(2, Cy2); + scope.set(3, Cy3); + scope.set(4, boxcheckC); - /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) - * Ensure that enough channels are available (HIDScope scope( 2 )) - * Finally, send all channels to the PC at once */ scope.send(); - /* To indicate that the function is working, the LED is toggled */ led = !led; } int main() { - /**Attach the 'sample' function to the timer 'sample_timer'. - * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz - */ - sample_timer.attach(&sample, 0.002); - - /*empty loop, sample() is executed periodically*/ + sample_timer.attach(&sample, 0.001); while(1) {} } \ No newline at end of file