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@4:fcada70891c5, 2016-10-27 (annotated)

Committer:
Iknowright
Date:
Thu Oct 27 14:55:11 2016 +0000
Revision:
4:fcada70891c5
Parent:
3:faed8b7f6542
Child:
5:51a28834cd5b
Filter for emg signals.

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);
Iknowright 4:fcada70891c5 8 Ticker sampleTicker;//Ticker that measures the EMG signal every 0,002 seconds and filters it
Iknowright 4:fcada70891c5 9 Ticker goTicker; //Ticker that checks if the signal threshold is reached each time a new filtered EMG signal comes in and determines if the motors are going to rotate
Iknowright 4:fcada70891c5 10 AnalogIn emgl(A1);//Labels are attached to the olimex shields the left tricep should obviously be connected to the should with an L label on it
Iknowright 4:fcada70891c5 11 AnalogIn emgr(A0);
Nickname 3:faed8b7f6542 12 DigitalOut ledG(LED_GREEN);
Nickname 3:faed8b7f6542 13 DigitalOut ledB(LED_BLUE);
Nickname 3:faed8b7f6542 14 DigitalOut ledR(LED_RED);
Iknowright 4:fcada70891c5 15 HIDScope scope(2); //scope has two ports for the two EMG signals
Iknowright 4:fcada70891c5 16
Iknowright 4:fcada70891c5 17 /*coefficients of each filter
Iknowright 4:fcada70891c5 18 lno = left tricep notch filter
Iknowright 4:fcada70891c5 19 lhf = left tricep high pass filter
Iknowright 4:fcada70891c5 20 llf = left tricep lowpass filter
Iknowright 4:fcada70891c5 21 same goes for rno etc.
Iknowright 4:fcada70891c5 22 */
Iknowright 4:fcada70891c5 23 double lno_b0 = 0.9911;
Iknowright 4:fcada70891c5 24 double lno_b1 = -1.6036;
Iknowright 4:fcada70891c5 25 double lno_b2 = 0.9911;
Iknowright 4:fcada70891c5 26 double lno_a1 = -1.603;
Iknowright 4:fcada70891c5 27 double lno_a2 = 0.9822;
Iknowright 4:fcada70891c5 28
Iknowright 4:fcada70891c5 29 double rno_b0 = 0.9911;
Iknowright 4:fcada70891c5 30 double rno_b1 = -1.6036;
Iknowright 4:fcada70891c5 31 double rno_b2 = 0.9911;
Iknowright 4:fcada70891c5 32 double rno_a1 = -1.603;
Iknowright 4:fcada70891c5 33 double rno_a2 = 0.9822;
Nickname 3:faed8b7f6542 34
Iknowright 4:fcada70891c5 35
Iknowright 4:fcada70891c5 36 double lhf_b0 = 0.9355;
Iknowright 4:fcada70891c5 37 double lhf_b1 = -1.8711;
Iknowright 4:fcada70891c5 38 double lhf_b2 = 0.9355;
Iknowright 4:fcada70891c5 39 double lhf_a1 = -1.8669;
Iknowright 4:fcada70891c5 40 double lhf_a2 = 0.8752;
Nickname 1:6081dc1ecd1f 41
Iknowright 4:fcada70891c5 42 double rhf_b0 = 0.9355;
Iknowright 4:fcada70891c5 43 double rhf_b1 = -1.8711;
Iknowright 4:fcada70891c5 44 double rhf_b2 = 0.9355;
Iknowright 4:fcada70891c5 45 double rhf_a1 = -1.8669;
Iknowright 4:fcada70891c5 46 double rhf_a2 = 0.8752;
Iknowright 4:fcada70891c5 47
Iknowright 4:fcada70891c5 48
Iknowright 4:fcada70891c5 49 double llf_b0 = 8.7656e-5;
Iknowright 4:fcada70891c5 50 double llf_b1 = 1.17531e-4;
Iknowright 4:fcada70891c5 51 double llf_b2 = 8.7656e-5;
Iknowright 4:fcada70891c5 52 double llf_a1 = -1.9733;
Iknowright 4:fcada70891c5 53 double llf_a2 = 0.9737;
Iknowright 0:41226c0fd285 54
Iknowright 4:fcada70891c5 55 double rlf_b0 = 8.7656e-5;
Iknowright 4:fcada70891c5 56 double rlf_b1 = 1.17531e-4;
Iknowright 4:fcada70891c5 57 double rlf_b2 = 8.7656e-5;
Iknowright 4:fcada70891c5 58 double rlf_a1 = -1.9733;
Iknowright 4:fcada70891c5 59 double rlf_a2 = 0.9737;
Iknowright 4:fcada70891c5 60
Iknowright 4:fcada70891c5 61
Iknowright 4:fcada70891c5 62 //starting values of the biquads of the corresponding filters
Iknowright 4:fcada70891c5 63 double lno_v1 = 0, lno_v2 = 0;
Iknowright 4:fcada70891c5 64 double lhf_v1 = 0, lhf_v2 = 0;
Iknowright 4:fcada70891c5 65 double llf_v1 = 0, llf_v2 = 0;
Iknowright 4:fcada70891c5 66
Iknowright 4:fcada70891c5 67 double rno_v1 = 0, rno_v2 = 0;
Iknowright 4:fcada70891c5 68 double rhf_v1 = 0, rhf_v2 = 0;
Iknowright 4:fcada70891c5 69 double rlf_v1 = 0, rlf_v2 = 0;
Nickname 1:6081dc1ecd1f 70
Iknowright 4:fcada70891c5 71 /* declaration of the outputs of each biquad.
Iknowright 4:fcada70891c5 72 the output of the previous biquad is the input for the next biquad.
Iknowright 4:fcada70891c5 73 so lno_y goes into lhf_y etc.
Iknowright 4:fcada70891c5 74 */
Iknowright 4:fcada70891c5 75 double lno_y;
Iknowright 4:fcada70891c5 76 double lhf_y;
Iknowright 4:fcada70891c5 77 double llf_y;
Iknowright 4:fcada70891c5 78 double lrect_y;
Iknowright 4:fcada70891c5 79 double rno_y;
Iknowright 4:fcada70891c5 80 double rhf_y;
Iknowright 4:fcada70891c5 81 double rlf_y;
Iknowright 4:fcada70891c5 82 double rrect_y;
Iknowright 0:41226c0fd285 83
Iknowright 4:fcada70891c5 84 // set the threshold value for the filtered signal
Iknowright 4:fcada70891c5 85 //if the signal exceeds this value the motors will start to rotate
Iknowright 4:fcada70891c5 86 const double threshold_value = 0.05;
Iknowright 4:fcada70891c5 87
Iknowright 4:fcada70891c5 88 /* declaration of each biquad
Iknowright 4:fcada70891c5 89 The coefficients will be filled in later on in void scopeSend
Iknowright 4:fcada70891c5 90 As said before the input of each biquad is the output of the previous one
Iknowright 4:fcada70891c5 91 The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal.
Iknowright 4:fcada70891c5 92 This is done for both left and right so this makes two chains of 3 biquads */
Iknowright 4:fcada70891c5 93
Iknowright 4:fcada70891c5 94 double biquad_lno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 95 const double b1 , const double b2 )
Nickname 1:6081dc1ecd1f 96 {
Nickname 3:faed8b7f6542 97 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 98 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 99 v2 = v1;
Nickname 3:faed8b7f6542 100 v1 = v;
Nickname 3:faed8b7f6542 101 return y;
Nickname 3:faed8b7f6542 102 }
Nickname 3:faed8b7f6542 103
Iknowright 4:fcada70891c5 104 double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 105 const double b1 , const double b2 )
Nickname 3:faed8b7f6542 106 {
Nickname 3:faed8b7f6542 107 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 108 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 109 v2 = v1;
Nickname 3:faed8b7f6542 110 v1 = v;
Nickname 3:faed8b7f6542 111 return y;
Nickname 1:6081dc1ecd1f 112 }
Iknowright 0:41226c0fd285 113
Iknowright 4:fcada70891c5 114 double biquad_llf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Iknowright 4:fcada70891c5 115 const double b1 , const double b2 )
Iknowright 4:fcada70891c5 116 {
Iknowright 4:fcada70891c5 117 double v = u - a1*v1 - a2*v2;
Iknowright 4:fcada70891c5 118 double y = b0*v + b1*v1 + b2*v2;
Iknowright 4:fcada70891c5 119 v2 = v1;
Iknowright 4:fcada70891c5 120 v1 = v;
Iknowright 4:fcada70891c5 121 return y;
Iknowright 4:fcada70891c5 122 }
Iknowright 4:fcada70891c5 123 double biquad_rno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Iknowright 4:fcada70891c5 124 const double b1 , const double b2 )
Iknowright 4:fcada70891c5 125 {
Iknowright 4:fcada70891c5 126 double v = u - a1*v1 - a2*v2;
Iknowright 4:fcada70891c5 127 double y = b0*v + b1*v1 + b2*v2;
Iknowright 4:fcada70891c5 128 v2 = v1;
Iknowright 4:fcada70891c5 129 v1 = v;
Iknowright 4:fcada70891c5 130 return y;
Iknowright 4:fcada70891c5 131 }
Iknowright 4:fcada70891c5 132
Iknowright 4:fcada70891c5 133 double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Iknowright 4:fcada70891c5 134 const double b1 , const double b2 )
Iknowright 4:fcada70891c5 135 {
Iknowright 4:fcada70891c5 136 double v = u - a1*v1 - a2*v2;
Iknowright 4:fcada70891c5 137 double y = b0*v + b1*v1 + b2*v2;
Iknowright 4:fcada70891c5 138 v2 = v1;
Iknowright 4:fcada70891c5 139 v1 = v;
Iknowright 4:fcada70891c5 140 return y;
Iknowright 4:fcada70891c5 141 }
Iknowright 4:fcada70891c5 142
Iknowright 4:fcada70891c5 143 double biquad_rlf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 144 const double b1 , const double b2 )
Nickname 3:faed8b7f6542 145 {
Nickname 3:faed8b7f6542 146 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 147 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 148 v2 = v1;
Nickname 3:faed8b7f6542 149 v1 = v;
Nickname 3:faed8b7f6542 150 return y;
Nickname 3:faed8b7f6542 151 }
Nickname 3:faed8b7f6542 152
Iknowright 4:fcada70891c5 153 /* function that calculates the filtered EMG signal from the raw EMG signal.
Iknowright 4:fcada70891c5 154 So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
Iknowright 4:fcada70891c5 155 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
Iknowright 4:fcada70891c5 156 The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
Nickname 3:faed8b7f6542 157 void scopeSend(void){
Iknowright 4:fcada70891c5 158 lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
Iknowright 4:fcada70891c5 159 lhf_y = biquad_lhf(lno_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
Iknowright 4:fcada70891c5 160 lrect_y = fabs(lhf_y);
Iknowright 4:fcada70891c5 161 llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
Iknowright 4:fcada70891c5 162 rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
Iknowright 4:fcada70891c5 163 rhf_y = biquad_rhf(rno_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
Iknowright 4:fcada70891c5 164 rrect_y = fabs(rhf_y);
Iknowright 4:fcada70891c5 165 rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;
Iknowright 4:fcada70891c5 166 scope.set(1, llf_y);
Iknowright 4:fcada70891c5 167 scope.set(0, rlf_y);
Nickname 3:faed8b7f6542 168 scope.send();
Nickname 3:faed8b7f6542 169
Nickname 3:faed8b7f6542 170 }
Iknowright 4:fcada70891c5 171
Iknowright 4:fcada70891c5 172 //function that compares the filtered EMG signal to the set threshold and determines what colour the led should be
Iknowright 4:fcada70891c5 173 //This led is for feedback purposes only and should obviously be replaced by the motors
Iknowright 4:fcada70891c5 174 void threshold(){
Iknowright 4:fcada70891c5 175 //If the right signal exceeds the threshold, the led should turn blue
Iknowright 4:fcada70891c5 176 if (rlf_y > threshold_value){
Iknowright 4:fcada70891c5 177 ledB = 0;
Iknowright 4:fcada70891c5 178 ledR = 1;
Iknowright 4:fcada70891c5 179 ledG = 1;
Nickname 3:faed8b7f6542 180 }
Iknowright 4:fcada70891c5 181 //If the left signal exceeds the threshold, the led should turn red
Iknowright 4:fcada70891c5 182 else if (llf_y > threshold_value){
Iknowright 4:fcada70891c5 183 ledB = 1;
Iknowright 4:fcada70891c5 184 ledR = 0;
Iknowright 4:fcada70891c5 185 ledG = 1;
Nickname 3:faed8b7f6542 186 }
Iknowright 4:fcada70891c5 187 // If both signals exceed the threshold, the led should turn green
Iknowright 4:fcada70891c5 188 else if (rlf_y&&llf_y > threshold_value){
Iknowright 4:fcada70891c5 189 ledB = 1;
Iknowright 4:fcada70891c5 190 ledR = 1;
Iknowright 4:fcada70891c5 191 ledG = 0;
Iknowright 4:fcada70891c5 192 }
Iknowright 4:fcada70891c5 193 //If no signal exceeds the threshold, the led should be off
Iknowright 4:fcada70891c5 194 else {
Iknowright 4:fcada70891c5 195 ledB=1;
Iknowright 4:fcada70891c5 196 ledG=1;
Iknowright 4:fcada70891c5 197 ledR=1;
Nickname 3:faed8b7f6542 198 }
Iknowright 4:fcada70891c5 199 }
Iknowright 4:fcada70891c5 200
Iknowright 0:41226c0fd285 201
Iknowright 0:41226c0fd285 202 int main(){
Iknowright 4:fcada70891c5 203
Iknowright 4:fcada70891c5 204
Nickname 1:6081dc1ecd1f 205
Iknowright 4:fcada70891c5 206 /*Attach the 'sample' function to the timer 'sample_timer'.
Iknowright 4:fcada70891c5 207 this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
Iknowright 4:fcada70891c5 208 emgSampleTicker.attach(&sample, 0.002);
Iknowright 4:fcada70891c5 209 empty loop, sample() is executed periodically
Iknowright 4:fcada70891c5 210 The same goes for the goTicker. It checks if the threshold is reached at the same rate new a new EMG signal comes in*/
Nickname 3:faed8b7f6542 211 sampleTicker.attach(scopeSend,0.002);
Nickname 3:faed8b7f6542 212 goTicker.attach(threshold,0.002);
Nickname 1:6081dc1ecd1f 213
Nickname 3:faed8b7f6542 214 while(1) {
Iknowright 4:fcada70891c5 215
Iknowright 4:fcada70891c5 216 }
Nickname 3:faed8b7f6542 217 }