Nick van den Berg / Mbed 2 deprecated EMGfilter24

Filter for EMG signals The signal will be filtered using a notch, highpass and lowpass filter. The filtered signal will be compared to a preset threshold and according to the strength of the signal the program will perform an action. In this case it will assign a colour to a led.

Dependencies:   HIDScope MODSERIAL mbed

Fork of EMGfilter24 by Steven Spoolder

main.cpp@3:faed8b7f6542, 2016-10-27 (annotated)

Committer:
Nickname
Date:
Thu Oct 27 08:14:12 2016 +0000
Revision:
3:faed8b7f6542
Parent:
2:6402a7071ba3
Child:
4:fcada70891c5
Trial without error;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Iknowright 0:41226c0fd285 1 #include "mbed.h"
Iknowright 0:41226c0fd285 2 #include "BiQuad.h"
Iknowright 0:41226c0fd285 3 #include "HIDScope.h"
Iknowright 0:41226c0fd285 4 #include "MODSERIAL.h"
Nickname 3:faed8b7f6542 5 #include <math.h>
Iknowright 0:41226c0fd285 6
Nickname 1:6081dc1ecd1f 7 MODSERIAL pc(USBTX, USBRX);
Nickname 3:faed8b7f6542 8 Ticker sampleTicker;
Nickname 3:faed8b7f6542 9 Ticker goTicker;
Nickname 1:6081dc1ecd1f 10 AnalogIn emg0(A0);
Nickname 1:6081dc1ecd1f 11 AnalogIn emg1(A1);
Nickname 3:faed8b7f6542 12 DigitalOut ledG(LED_GREEN);
Nickname 3:faed8b7f6542 13 DigitalOut ledB(LED_BLUE);
Nickname 3:faed8b7f6542 14 DigitalOut ledR(LED_RED);
Nickname 3:faed8b7f6542 15 HIDScope scope(2);
Nickname 3:faed8b7f6542 16
Nickname 3:faed8b7f6542 17 double no_b0 = 0.9911;
Nickname 3:faed8b7f6542 18 double no_b1 = -1.6036;
Nickname 3:faed8b7f6542 19 double no_b2 = 0.9911;
Nickname 3:faed8b7f6542 20 double no_a1 = -1.6036;
Nickname 3:faed8b7f6542 21 double no_a2 = 0.9822;
Nickname 1:6081dc1ecd1f 22
Nickname 1:6081dc1ecd1f 23 double hf_b0 = 0.9355;
Nickname 1:6081dc1ecd1f 24 double hf_b1 = -1.8711;
Nickname 1:6081dc1ecd1f 25 double hf_b2 = 0.9355;
Nickname 1:6081dc1ecd1f 26 double hf_a1 = -1.8669;
Nickname 1:6081dc1ecd1f 27 double hf_a2 = 0.8752;
Iknowright 0:41226c0fd285 28
Nickname 1:6081dc1ecd1f 29 double lf_b0 = 8.7656e-5;
Nickname 1:6081dc1ecd1f 30 double lf_b1 = 1.17531e-4;
Nickname 1:6081dc1ecd1f 31 double lf_b2 = 8.7656e-5;
Nickname 1:6081dc1ecd1f 32 double lf_a1 = -1.9733;
Nickname 1:6081dc1ecd1f 33 double lf_a2 = 0.9737;
Nickname 1:6081dc1ecd1f 34
Nickname 3:faed8b7f6542 35 double no_v1 = 0, no_v2 = 0;
Nickname 3:faed8b7f6542 36 double hf_v1 = 0, hf_v2 = 0;
Nickname 3:faed8b7f6542 37 double lf_v1 = 0, lf_v2 = 0;
Nickname 3:faed8b7f6542 38 double no_y;
Nickname 3:faed8b7f6542 39 double lf_y;
Nickname 3:faed8b7f6542 40 double hf_y;
Nickname 3:faed8b7f6542 41 double rect_y;
Nickname 3:faed8b7f6542 42 int go = 0;
Nickname 3:faed8b7f6542 43 const double threshold_value = 0.13;
Iknowright 0:41226c0fd285 44
Nickname 3:faed8b7f6542 45 double biquad_no(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 46 const double b1 , const double b2 )
Nickname 1:6081dc1ecd1f 47 {
Nickname 3:faed8b7f6542 48 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 49 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 50 v2 = v1;
Nickname 3:faed8b7f6542 51 v1 = v;
Nickname 3:faed8b7f6542 52 return y;
Nickname 3:faed8b7f6542 53 }
Nickname 3:faed8b7f6542 54
Nickname 3:faed8b7f6542 55 double biquad_hf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 56 const double b1 , const double b2 )
Nickname 3:faed8b7f6542 57 {
Nickname 3:faed8b7f6542 58 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 59 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 60 v2 = v1;
Nickname 3:faed8b7f6542 61 v1 = v;
Nickname 3:faed8b7f6542 62 return y;
Nickname 1:6081dc1ecd1f 63 }
Iknowright 0:41226c0fd285 64
Nickname 3:faed8b7f6542 65 double biquad_lf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 66 const double b1 , const double b2 )
Nickname 3:faed8b7f6542 67 {
Nickname 3:faed8b7f6542 68 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 69 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 70 v2 = v1;
Nickname 3:faed8b7f6542 71 v1 = v;
Nickname 3:faed8b7f6542 72 return y;
Nickname 3:faed8b7f6542 73 }
Nickname 3:faed8b7f6542 74
Nickname 3:faed8b7f6542 75 void scopeSend(void){
Nickname 3:faed8b7f6542 76 no_y = biquad_no(emg0.read(), no_v1, no_v2, no_a1, no_a2, no_b0, no_b1, no_b2);
Nickname 3:faed8b7f6542 77 hf_y = biquad_hf(no_y, hf_v1, hf_v2, hf_a1, hf_a2, hf_b0, hf_b1, hf_b2);
Nickname 3:faed8b7f6542 78 rect_y = fabs(hf_y);
Nickname 3:faed8b7f6542 79 lf_y = biquad_lf(rect_y, lf_v1, lf_v2, lf_a1, lf_a2, lf_b0, lf_b1, lf_b2)/0.2;
Nickname 3:faed8b7f6542 80 scope.set(0, emg0.read());
Nickname 3:faed8b7f6542 81 scope.set(1, lf_y);
Nickname 3:faed8b7f6542 82 scope.send();
Nickname 3:faed8b7f6542 83
Nickname 3:faed8b7f6542 84 }
Nickname 3:faed8b7f6542 85
Nickname 3:faed8b7f6542 86 void threshold(double lf_y, const double threshold_value){
Nickname 3:faed8b7f6542 87 if (lf_y > threshold_value){
Nickname 3:faed8b7f6542 88 go = !go
Nickname 3:faed8b7f6542 89 }
Nickname 3:faed8b7f6542 90 /* if (lf_y > threshold_value){
Nickname 3:faed8b7f6542 91 ledB = !ledB;
Nickname 3:faed8b7f6542 92 }
Nickname 3:faed8b7f6542 93 else{
Nickname 3:faed8b7f6542 94 ledB = ledB;
Nickname 3:faed8b7f6542 95 }
Nickname 3:faed8b7f6542 96 }
Nickname 3:faed8b7f6542 97 */
Iknowright 0:41226c0fd285 98
Iknowright 0:41226c0fd285 99 int main(){
Nickname 1:6081dc1ecd1f 100
Nickname 1:6081dc1ecd1f 101 /**Attach the 'sample' function to the timer 'sample_timer'.
Nickname 1:6081dc1ecd1f 102 * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
Nickname 1:6081dc1ecd1f 103 */
Nickname 3:faed8b7f6542 104 // emgSampleTicker.attach(&sample, 0.002);
Nickname 1:6081dc1ecd1f 105 // empty loop, sample() is executed periodically
Nickname 3:faed8b7f6542 106
Nickname 3:faed8b7f6542 107 sampleTicker.attach(scopeSend,0.002);
Nickname 3:faed8b7f6542 108 goTicker.attach(threshold,0.002);
Nickname 1:6081dc1ecd1f 109
Nickname 3:faed8b7f6542 110 while(1) {
Nickname 3:faed8b7f6542 111 if (go == 0){
Nickname 3:faed8b7f6542 112 ledB = ledB;
Nickname 3:faed8b7f6542 113 }
Nickname 3:faed8b7f6542 114 else{
Nickname 3:faed8b7f6542 115 ledB = !ledB;
Nickname 1:6081dc1ecd1f 116 }
Nickname 3:faed8b7f6542 117 }