Emg filter function script for a uni project. Made by Teun van der Molen
Dependencies: HIDScope MODSERIAL mbed
Fork of frdm_EMG by
main.cpp@11:d8dd024f9784, 2015-10-19 (annotated)
- Committer:
- teunman
- Date:
- Mon Oct 19 09:58:49 2015 +0000
- Revision:
- 11:d8dd024f9784
- Parent:
- 10:b11eacb391ea
latest version. Cleaned up some documentation not much else;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
teunman | 0:674026fdd982 | 1 | #include "mbed.h" |
teunman | 0:674026fdd982 | 2 | #include "HIDScope.h" |
teunman | 0:674026fdd982 | 3 | #include "math.h" |
teunman | 0:674026fdd982 | 4 | // Define the HIDScope and Ticker object |
teunman | 11:d8dd024f9784 | 5 | HIDScope scope(4); |
teunman | 10:b11eacb391ea | 6 | |
teunman | 0:674026fdd982 | 7 | Ticker scopeTimer; |
teunman | 11:d8dd024f9784 | 8 | Ticker FilterTicker; |
teunman | 10:b11eacb391ea | 9 | |
teunman | 10:b11eacb391ea | 10 | |
teunman | 11:d8dd024f9784 | 11 | ///////////////////////////////////////////////////MBED PORTS//////////////////////////////////////////////////////////////////// |
teunman | 11:d8dd024f9784 | 12 | |
teunman | 8:54f0a76d35f4 | 13 | DigitalOut light(LED2); |
teunman | 8:54f0a76d35f4 | 14 | DigitalIn knop(SW3); |
teunman | 10:b11eacb391ea | 15 | AnalogIn an_in(A0); //The input from the emg board, make sure that the jumper pin on the EMG board corresponds with the value in the brackets here!!! (see bb pptx for details on jumper placement, default is A0) |
teunman | 10:b11eacb391ea | 16 | |
teunman | 10:b11eacb391ea | 17 | |
teunman | 11:d8dd024f9784 | 18 | /////////////////////////////////////////FILTER VALUES/////////////////////////////////////////////////////////////////////////////////// |
teunman | 10:b11eacb391ea | 19 | |
teunman | 11:d8dd024f9784 | 20 | const float lag = 2500; //NO. of data points for moving average filter |
teunman | 11:d8dd024f9784 | 21 | float values [2500] = { }; // The array that stores the values for Movavg !!!!! the value between "[]" MUST BE THE SAME AS LAG!!!!!!!!! (needs to be a numerical value so can't fill in lag here :( |
teunman | 11:d8dd024f9784 | 22 | int n_avg = 0; // Counter for movavg |
teunman | 11:d8dd024f9784 | 23 | float y_avg = 0; // value for movavg |
teunman | 11:d8dd024f9784 | 24 | |
teunman | 11:d8dd024f9784 | 25 | double ffy = 0; //movavg filterd output |
teunman | 11:d8dd024f9784 | 26 | |
teunman | 10:b11eacb391ea | 27 | double c = 5; //calibration value (in volts) |
teunman | 8:54f0a76d35f4 | 28 | double n = 0; // counter for calibration |
teunman | 8:54f0a76d35f4 | 29 | bool calib = false; //changes into false when calibration is done (reset program for new calibration); |
teunman | 10:b11eacb391ea | 30 | double v1=0, v2=0, u=0, y=0, fy=0, fv1=0, fv2=0, fu=0, ny=0, nv1=0, nv2=0, nu=0;; |
teunman | 10:b11eacb391ea | 31 | |
teunman | 10:b11eacb391ea | 32 | |
teunman | 10:b11eacb391ea | 33 | //Notch filter (n stand for notch filter (this is in fact the very first filter but is rarely changed) to alter this filter add "n" to all coefficents example: nb0,nb1 etc.) |
teunman | 10:b11eacb391ea | 34 | const double nb0 = 0.8206743576788857,nb1 = -1.5610153912536877,nb2 = 0.8206743576788857,na1 = -1.5610153912536877,na2 = 0.6413487153577715; //Notch filter Fc = 50 Hz Fs 1000Hz |
teunman | 10:b11eacb391ea | 35 | |
teunman | 10:b11eacb391ea | 36 | |
teunman | 10:b11eacb391ea | 37 | // 100 Hz filters (obsolete) |
teunman | 1:75f61e111ed0 | 38 | |
teunman | 5:56725d9362ee | 39 | //const double b0 = 0.9999999999999999,b1 = 1.9999999999999998,b2 = 0.9999999999999999, a1 = 1.9999999999999998 ,a2 = 0.9999999999999998; //low-pass Fc = 50hz fs = 100hz |
teunman | 5:56725d9362ee | 40 | //const double fb0 = 0.02008333102602092 ,fb1 = 0.04016666205204184 ,fb2 = 0.02008333102602092, fa1 = -1.5610153912536877 ,fa2 = 0.6413487153577715; //low-pass Fc = 5hz fs = 100hz |
teunman | 5:56725d9362ee | 41 | //const double b0 = 0.8005910266528649,b1 = -1.6011820533057297,b2 = 0.8005910266528649,a1 = -1.5610153912536877,a2 = 0.6413487153577715; //high-pass Fc = 5hz fs = 100hz |
teunman | 5:56725d9362ee | 42 | //const double b0 = 0.007820199259120319,b1 = 0.015640398518240638,b2 = 0.007820199259120319,a1 = -1.7347238224240125,a2 = 0.7660046194604936; //low-pass Fc = 3hz fs = 100hz |
teunman | 5:56725d9362ee | 43 | //const double b0 = 0.0009446914586925257,b1 = 0.0018893829173850514,b2 = 0.0009446914586925257,a1 = -1.911196288237583,a2 = 0.914975054072353; //low-pass Fc = 1hz fs = 100hz |
teunman | 5:56725d9362ee | 44 | //const double b0 = 0.956542835577484, b1 = -1.913085671154968, b2 = 0.956542835577484, a1 = -1.911196288237583, a2 = 0.914975054072353; //high-pass Fc = 1hz fs = 100hz |
teunman | 3:499c71ca30a0 | 45 | |
teunman | 10:b11eacb391ea | 46 | |
teunman | 10:b11eacb391ea | 47 | |
teunman | 10:b11eacb391ea | 48 | // 1000 Hz filters (f stands for the seccond filter, so make sure the first coefiecients are b0,b1 etc. and the seccond filter is fb0,fb1,fb2 etc.) |
teunman | 10:b11eacb391ea | 49 | |
teunman | 10:b11eacb391ea | 50 | //const double b0 = 0.00008765553769759188,b1 = 0.00017531107539518376,b2 = 0.00008765553769759188,a1 = -1.9733440008737442,a2 = 0.9736946230245347;//low-pass Fc = 3Hz fs = 1000hz |
teunman | 10:b11eacb391ea | 51 | const double fb0 = 0.00034604125149151127,fb1 = 0.0006920825029830225, fb2 = 0.00034604125149151127 ,fa1 = -1.9466970561224466 ,fa2 = 0.9480812211284125; //low-pass Fc = 6Hz fs = 1000hz |
teunman | 7:43f2f7039841 | 52 | //const double fb0 = 0.9149684297741606, fb1 = -1.8299368595483212, fb2 = 0.9149684297741606 ,fa1 = -1.8226935021735358, fa2 = 0.8371802169231065; // High-pass Fc = 20Hz fs = 1000hz |
teunman | 5:56725d9362ee | 53 | //const double fb0 = 0.8948577513857248 ,fb1 = -1.7897155027714495, fb2 = 0.8948577513857248,fa1 = -1.7786300789392977,fa2 = 0.8008009266036016; // High-pass Fc = 25Hz fs = 1000Hz |
teunman | 6:4cbf5c66e2fb | 54 | //const double b0 = 0.005542711916075981,b1 = 0.011085423832151962,b2 = 0.005542711916075981,a1 = -1.7786300789392977,a2 = 0.8008009266036016; //Low-pass Fc = 25Hz fs=1000Hz |
teunman | 6:4cbf5c66e2fb | 55 | //const double fb0 = 0.9780302754084559,fb1 = -1.9560605508169118,fb2 = 0.9780302754084559,fa1 = -1.9555778328194147,fa2 = 0.9565432688144089; //high-pass Fc = 6hz fs = 1000hz |
teunman | 10:b11eacb391ea | 56 | const double b0 = 0.9911535113858849,b1 = -1.9823070227717698,b2 = 0.9911535113858849,a1 = -1.9822287623675816,a2 = 0.982385283175958; //High-pass Fc = 2Hz fs = 1000Hz |
teunman | 6:4cbf5c66e2fb | 57 | //const double fb0 = 0.0036216786873927774,fb1 = 0.007243357374785555,fb2 = 0.0036216786873927774,fa1 = -1.8226935021735358,fa2 = 0.8371802169231065; ///Low-pass Fc = 20 Hz Fs = 1000Hz |
teunman | 6:4cbf5c66e2fb | 58 | |
teunman | 3:499c71ca30a0 | 59 | |
teunman | 11:d8dd024f9784 | 60 | ////////////////////////////////////////////////////////////////////HID SCOPE FUNCTION////////////////////////////////////////////////////////////////////////////// |
teunman | 10:b11eacb391ea | 61 | |
teunman | 3:499c71ca30a0 | 62 | |
teunman | 10:b11eacb391ea | 63 | |
teunman | 10:b11eacb391ea | 64 | // The data read and send function for HIDscope (can send any value to the HIDscope for read out) |
teunman | 3:499c71ca30a0 | 65 | void scopeSend() |
teunman | 3:499c71ca30a0 | 66 | { |
teunman | 11:d8dd024f9784 | 67 | scope.set(0,an_in); |
teunman | 4:1baefd1397d6 | 68 | scope.set(1,fy); |
teunman | 11:d8dd024f9784 | 69 | scope.set(2,ffy); |
teunman | 11:d8dd024f9784 | 70 | scope.set(3,c); |
teunman | 3:499c71ca30a0 | 71 | scope.send(); |
teunman | 8:54f0a76d35f4 | 72 | |
teunman | 10:b11eacb391ea | 73 | |
teunman | 8:54f0a76d35f4 | 74 | |
teunman | 8:54f0a76d35f4 | 75 | |
teunman | 3:499c71ca30a0 | 76 | } |
teunman | 3:499c71ca30a0 | 77 | |
teunman | 11:d8dd024f9784 | 78 | |
teunman | 11:d8dd024f9784 | 79 | ///////////////////////////////////////////////////////////////////////FILTER CASCADE//////////////////////////////////////////////////////////////////////// |
teunman | 11:d8dd024f9784 | 80 | |
teunman | 10:b11eacb391ea | 81 | void computeBiquad(){ //The filter function (is called at the same frequency of the scope function (not sure if timing is fully correct here) and does the filter calculations) |
teunman | 7:43f2f7039841 | 82 | |
teunman | 10:b11eacb391ea | 83 | double nv = an_in - na1*nv1 - na2*nv2; //notch filter at 50Hz for laptop adapter ny is the filtered output |
teunman | 10:b11eacb391ea | 84 | ny= nb0*nv + nb1*nv1 +nb2*nv2; |
teunman | 10:b11eacb391ea | 85 | nv2=nv1; |
teunman | 10:b11eacb391ea | 86 | nv1=nv; |
teunman | 7:43f2f7039841 | 87 | |
teunman | 10:b11eacb391ea | 88 | double v = ny - a1*v1 - a2*v2; //First filter (see filters to check values) y is the filtered output |
teunman | 1:75f61e111ed0 | 89 | y= b0*v + b1*v1 +b2*v2; |
teunman | 1:75f61e111ed0 | 90 | v2=v1; |
teunman | 1:75f61e111ed0 | 91 | v1=v; |
teunman | 4:1baefd1397d6 | 92 | |
teunman | 10:b11eacb391ea | 93 | y = abs(y); //Rectifier y is the rectified output |
teunman | 7:43f2f7039841 | 94 | |
teunman | 6:4cbf5c66e2fb | 95 | |
teunman | 10:b11eacb391ea | 96 | double fv = y - fa1*fv1 - fa2*fv2; //Second Filter (see filters to check values) fy is the filtered output |
teunman | 4:1baefd1397d6 | 97 | fy= fb0*fv + fb1*fv1 + fb2*fv2; |
teunman | 4:1baefd1397d6 | 98 | fv2=fv1; |
teunman | 4:1baefd1397d6 | 99 | fv1=fv; |
teunman | 7:43f2f7039841 | 100 | |
teunman | 10:b11eacb391ea | 101 | //fy = abs(fy); //Optional final rectifier (default should be off) |
teunman | 11:d8dd024f9784 | 102 | |
teunman | 11:d8dd024f9784 | 103 | values [n_avg] = fy; //Moving Average filter (see filters to check lag etc.) |
teunman | 11:d8dd024f9784 | 104 | n_avg = n_avg + 1; |
teunman | 11:d8dd024f9784 | 105 | |
teunman | 11:d8dd024f9784 | 106 | if (n_avg == lag){ |
teunman | 11:d8dd024f9784 | 107 | |
teunman | 11:d8dd024f9784 | 108 | for (int n_divide = 0 ;n_divide <= lag; n_divide = n_divide + 1){ |
teunman | 11:d8dd024f9784 | 109 | y_avg = y_avg + values [n_divide]; |
teunman | 11:d8dd024f9784 | 110 | |
teunman | 11:d8dd024f9784 | 111 | } |
teunman | 11:d8dd024f9784 | 112 | |
teunman | 11:d8dd024f9784 | 113 | n_avg = 0; |
teunman | 11:d8dd024f9784 | 114 | ffy = y_avg/lag; |
teunman | 11:d8dd024f9784 | 115 | y_avg = 0; |
teunman | 11:d8dd024f9784 | 116 | } |
teunman | 11:d8dd024f9784 | 117 | |
teunman | 11:d8dd024f9784 | 118 | |
teunman | 11:d8dd024f9784 | 119 | |
teunman | 11:d8dd024f9784 | 120 | } //end compute biquad |
teunman | 3:499c71ca30a0 | 121 | |
teunman | 0:674026fdd982 | 122 | |
teunman | 8:54f0a76d35f4 | 123 | |
teunman | 8:54f0a76d35f4 | 124 | |
teunman | 8:54f0a76d35f4 | 125 | |
teunman | 11:d8dd024f9784 | 126 | ////////////////////////////////////////////////////////////////////////MAIN FUNCTION///////////////////////////////////////////////////////////// |
teunman | 8:54f0a76d35f4 | 127 | |
teunman | 8:54f0a76d35f4 | 128 | |
teunman | 0:674026fdd982 | 129 | int main() |
teunman | 0:674026fdd982 | 130 | { |
teunman | 11:d8dd024f9784 | 131 | light = 1; //Light is off by default |
teunman | 11:d8dd024f9784 | 132 | |
teunman | 11:d8dd024f9784 | 133 | |
teunman | 11:d8dd024f9784 | 134 | /////////////////////////////////////////////////////////////////TICKERS/////////////////////////////////////////////////////////////////////// |
teunman | 8:54f0a76d35f4 | 135 | |
teunman | 5:56725d9362ee | 136 | // Attach the data read and send function at 1000 Hz |
teunman | 5:56725d9362ee | 137 | scopeTimer.attach_us(&scopeSend, 1e5); |
teunman | 10:b11eacb391ea | 138 | // Attach the filter calculation function at 1000 Hz |
teunman | 11:d8dd024f9784 | 139 | FilterTicker.attach(&computeBiquad,0.0001f); |
teunman | 10:b11eacb391ea | 140 | |
teunman | 10:b11eacb391ea | 141 | |
teunman | 10:b11eacb391ea | 142 | |
teunman | 10:b11eacb391ea | 143 | while(1) { //the everlasting while loop |
teunman | 10:b11eacb391ea | 144 | |
teunman | 10:b11eacb391ea | 145 | |
teunman | 11:d8dd024f9784 | 146 | /////////////////////////////////////////////////////CALIBRATION AND TRIGGER //////////////////////////////////////////// |
teunman | 11:d8dd024f9784 | 147 | |
teunman | 11:d8dd024f9784 | 148 | if (ffy >= c and calib == false){ //if there is no calibration going on and the filtered emg is above calibration the light goes on |
teunman | 10:b11eacb391ea | 149 | light = 0; |
teunman | 10:b11eacb391ea | 150 | } |
teunman | 8:54f0a76d35f4 | 151 | |
teunman | 8:54f0a76d35f4 | 152 | |
teunman | 10:b11eacb391ea | 153 | else if (knop == 0){ //pressing the button starts calibration |
teunman | 10:b11eacb391ea | 154 | |
teunman | 10:b11eacb391ea | 155 | c = 0; |
teunman | 10:b11eacb391ea | 156 | n = 0; |
teunman | 10:b11eacb391ea | 157 | light = 0; //light is on during calibration |
teunman | 10:b11eacb391ea | 158 | calib = true; |
teunman | 10:b11eacb391ea | 159 | |
teunman | 10:b11eacb391ea | 160 | for(;calib == true and n <= 20.00; n = n + 1) { //this loop ads the calibration value from the output of the filter, this also decides the time of calibration (see wait in combination with the n maximum) |
teunman | 9:2b9240084724 | 161 | |
teunman | 11:d8dd024f9784 | 162 | c = (c + ffy); |
teunman | 10:b11eacb391ea | 163 | wait(0.1); |
teunman | 9:2b9240084724 | 164 | |
teunman | 10:b11eacb391ea | 165 | }//end for |
teunman | 9:2b9240084724 | 166 | |
teunman | 9:2b9240084724 | 167 | |
teunman | 9:2b9240084724 | 168 | |
teunman | 10:b11eacb391ea | 169 | light = 1; // light is off for a while after calibration |
teunman | 10:b11eacb391ea | 170 | calib = false; // shuts of calibration |
teunman | 11:d8dd024f9784 | 171 | c = (c/n) * 0.3; //take the average of the emg output during calibration and set the calibration value |
teunman | 10:b11eacb391ea | 172 | |
teunman | 9:2b9240084724 | 173 | |
teunman | 10:b11eacb391ea | 174 | wait(5); // you can only calibrate every this amount of seconds |
teunman | 10:b11eacb391ea | 175 | } //end else if |
teunman | 10:b11eacb391ea | 176 | |
teunman | 8:54f0a76d35f4 | 177 | else{ |
teunman | 10:b11eacb391ea | 178 | light = 1; //light stays off by default |
teunman | 6:4cbf5c66e2fb | 179 | } |
teunman | 10:b11eacb391ea | 180 | |
teunman | 10:b11eacb391ea | 181 | |
teunman | 10:b11eacb391ea | 182 | } //END while |
teunman | 10:b11eacb391ea | 183 | }//END main |