Emg filter function script for a uni project. Made by Teun van der Molen
Dependencies: HIDScope MODSERIAL mbed
main.cpp@9:2b9240084724, 2015-09-23 (annotated)
- Committer:
- teunman
- Date:
- Wed Sep 23 15:53:10 2015 +0000
- Revision:
- 9:2b9240084724
- Parent:
- 8:54f0a76d35f4
- Child:
- 10:b11eacb391ea
same as last but with working calibration (fully flex muscle for 2 seconds and press button)
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 | 4:1baefd1397d6 | 5 | HIDScope scope(3); |
teunman | 0:674026fdd982 | 6 | Ticker scopeTimer; |
teunman | 1:75f61e111ed0 | 7 | Ticker biquadTicker; |
teunman | 8:54f0a76d35f4 | 8 | DigitalOut light(LED2); |
teunman | 8:54f0a76d35f4 | 9 | DigitalIn knop(SW3); |
teunman | 9:2b9240084724 | 10 | int tel = 0; // timer thing for caliration length (see for loop in main) |
teunman | 8:54f0a76d35f4 | 11 | double c=5; //calibration value |
teunman | 8:54f0a76d35f4 | 12 | double n = 0; // counter for calibration |
teunman | 8:54f0a76d35f4 | 13 | bool calib = false; //changes into false when calibration is done (reset program for new calibration); |
teunman | 4:1baefd1397d6 | 14 | double v1=0, v2=0, u=0, y=0, fy=0, fv1=0, fv2=0, fu=0; |
teunman | 1:75f61e111ed0 | 15 | |
teunman | 5:56725d9362ee | 16 | //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 | 17 | //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 | 18 | //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 | 19 | //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 | 20 | //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 | 21 | //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 | 22 | |
teunman | 7:43f2f7039841 | 23 | const double b0 = 0.00008765553769759188,b1 = 0.00017531107539518376,b2 = 0.00008765553769759188,a1 = -1.9733440008737442,a2 = 0.9736946230245347;//low-pass Fc = 3Hz fs = 1000hz |
teunman | 7:43f2f7039841 | 24 | //const double b0 = 0.00034604125149151127,b1 = 0.0006920825029830225, b2 = 0.00034604125149151127 ,a1 = -1.9466970561224466 ,a2 = 0.9480812211284125; //low-pass Fc = 6Hz fs = 1000hz |
teunman | 7:43f2f7039841 | 25 | //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 | 26 | //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 | 27 | //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 | 28 | //const double fb0 = 0.9780302754084559,fb1 = -1.9560605508169118,fb2 = 0.9780302754084559,fa1 = -1.9555778328194147,fa2 = 0.9565432688144089; //high-pass Fc = 6hz fs = 1000hz |
teunman | 7:43f2f7039841 | 29 | const double fb0 = 0.9911535113858849,fb1 = -1.9823070227717698,fb2 = 0.9911535113858849,fa1 = -1.9822287623675816,fa2 = 0.982385283175958; //High-pass Fc = 2Hz fs = 1000Hz |
teunman | 6:4cbf5c66e2fb | 30 | //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 | 31 | |
teunman | 3:499c71ca30a0 | 32 | |
teunman | 3:499c71ca30a0 | 33 | |
teunman | 3:499c71ca30a0 | 34 | // Read the analog input |
teunman | 3:499c71ca30a0 | 35 | AnalogIn an_in(A0); |
teunman | 3:499c71ca30a0 | 36 | |
teunman | 3:499c71ca30a0 | 37 | AnalogOut an_out(DAC0_OUT); |
teunman | 3:499c71ca30a0 | 38 | // The data read and send function |
teunman | 3:499c71ca30a0 | 39 | void scopeSend() |
teunman | 3:499c71ca30a0 | 40 | { |
teunman | 8:54f0a76d35f4 | 41 | scope.set(0,c); |
teunman | 4:1baefd1397d6 | 42 | scope.set(1,fy); |
teunman | 8:54f0a76d35f4 | 43 | scope.set(2,n); |
teunman | 3:499c71ca30a0 | 44 | |
teunman | 3:499c71ca30a0 | 45 | scope.send(); |
teunman | 8:54f0a76d35f4 | 46 | |
teunman | 9:2b9240084724 | 47 | //while(calib == true and n <= 200.00) { |
teunman | 8:54f0a76d35f4 | 48 | |
teunman | 9:2b9240084724 | 49 | // c = c + fy; |
teunman | 8:54f0a76d35f4 | 50 | |
teunman | 9:2b9240084724 | 51 | //n ++; |
teunman | 8:54f0a76d35f4 | 52 | |
teunman | 9:2b9240084724 | 53 | //} |
teunman | 8:54f0a76d35f4 | 54 | |
teunman | 8:54f0a76d35f4 | 55 | |
teunman | 3:499c71ca30a0 | 56 | } |
teunman | 3:499c71ca30a0 | 57 | |
teunman | 3:499c71ca30a0 | 58 | void computeBiquad(){ |
teunman | 7:43f2f7039841 | 59 | |
teunman | 7:43f2f7039841 | 60 | |
teunman | 7:43f2f7039841 | 61 | |
teunman | 3:499c71ca30a0 | 62 | double v = an_in - a1*v1 - a2*v2; |
teunman | 1:75f61e111ed0 | 63 | y= b0*v + b1*v1 +b2*v2; |
teunman | 1:75f61e111ed0 | 64 | v2=v1; |
teunman | 1:75f61e111ed0 | 65 | v1=v; |
teunman | 4:1baefd1397d6 | 66 | |
teunman | 7:43f2f7039841 | 67 | y = abs(y); |
teunman | 7:43f2f7039841 | 68 | |
teunman | 6:4cbf5c66e2fb | 69 | |
teunman | 4:1baefd1397d6 | 70 | double fv = y - fa1*fv1 - fa2*fv2; |
teunman | 4:1baefd1397d6 | 71 | fy= fb0*fv + fb1*fv1 + fb2*fv2; |
teunman | 4:1baefd1397d6 | 72 | fv2=fv1; |
teunman | 4:1baefd1397d6 | 73 | fv1=fv; |
teunman | 7:43f2f7039841 | 74 | |
teunman | 7:43f2f7039841 | 75 | fy = abs(fy); |
teunman | 1:75f61e111ed0 | 76 | } |
teunman | 3:499c71ca30a0 | 77 | |
teunman | 0:674026fdd982 | 78 | |
teunman | 8:54f0a76d35f4 | 79 | |
teunman | 8:54f0a76d35f4 | 80 | |
teunman | 8:54f0a76d35f4 | 81 | |
teunman | 8:54f0a76d35f4 | 82 | |
teunman | 8:54f0a76d35f4 | 83 | |
teunman | 8:54f0a76d35f4 | 84 | |
teunman | 0:674026fdd982 | 85 | int main() |
teunman | 0:674026fdd982 | 86 | { |
teunman | 6:4cbf5c66e2fb | 87 | light = 1; |
teunman | 8:54f0a76d35f4 | 88 | |
teunman | 5:56725d9362ee | 89 | // Attach the data read and send function at 1000 Hz |
teunman | 5:56725d9362ee | 90 | scopeTimer.attach_us(&scopeSend, 1e5); |
teunman | 8:54f0a76d35f4 | 91 | //scopeTimer.attach_us(&calibrate, 1e5); |
teunman | 5:56725d9362ee | 92 | biquadTicker.attach(&computeBiquad,0.0001f); |
teunman | 3:499c71ca30a0 | 93 | //float i = 1; |
teunman | 0:674026fdd982 | 94 | while(1) { |
teunman | 8:54f0a76d35f4 | 95 | if (fy >= c and calib == false){ |
teunman | 6:4cbf5c66e2fb | 96 | light = 0; |
teunman | 6:4cbf5c66e2fb | 97 | } |
teunman | 8:54f0a76d35f4 | 98 | |
teunman | 8:54f0a76d35f4 | 99 | |
teunman | 8:54f0a76d35f4 | 100 | else if (knop == 0){ |
teunman | 8:54f0a76d35f4 | 101 | c = 0; |
teunman | 8:54f0a76d35f4 | 102 | n = 0; |
teunman | 9:2b9240084724 | 103 | //for (tel = 0; tel == 20; tel ++){ |
teunman | 9:2b9240084724 | 104 | light = 0; |
teunman | 9:2b9240084724 | 105 | calib = true; |
teunman | 9:2b9240084724 | 106 | for(;calib == true and n <= 20.00; n = n + 1) { |
teunman | 9:2b9240084724 | 107 | |
teunman | 9:2b9240084724 | 108 | c = c + fy; |
teunman | 9:2b9240084724 | 109 | |
teunman | 9:2b9240084724 | 110 | //n ++; |
teunman | 9:2b9240084724 | 111 | |
teunman | 9:2b9240084724 | 112 | wait(0.1); |
teunman | 9:2b9240084724 | 113 | |
teunman | 9:2b9240084724 | 114 | } //while |
teunman | 9:2b9240084724 | 115 | |
teunman | 9:2b9240084724 | 116 | // } |
teunman | 9:2b9240084724 | 117 | |
teunman | 9:2b9240084724 | 118 | |
teunman | 9:2b9240084724 | 119 | |
teunman | 8:54f0a76d35f4 | 120 | light = 1; |
teunman | 9:2b9240084724 | 121 | calib = false; // shuts of calibration |
teunman | 9:2b9240084724 | 122 | c = c/n; //take the average |
teunman | 9:2b9240084724 | 123 | tel = 0; |
teunman | 9:2b9240084724 | 124 | |
teunman | 9:2b9240084724 | 125 | wait(5); // you can only calibrate every this amount of seconds |
teunman | 9:2b9240084724 | 126 | } //if |
teunman | 8:54f0a76d35f4 | 127 | |
teunman | 8:54f0a76d35f4 | 128 | |
teunman | 8:54f0a76d35f4 | 129 | else{ |
teunman | 8:54f0a76d35f4 | 130 | light = 1; |
teunman | 6:4cbf5c66e2fb | 131 | } |
teunman | 0:674026fdd982 | 132 | } |
teunman | 0:674026fdd982 | 133 | } |