script zover
Dependencies: HIDScope MODSERIAL mbed
main.cpp@11:91d827c62a50, 2014-10-22 (annotated)
- Committer:
- wiesdat
- Date:
- Wed Oct 22 09:24:13 2014 +0000
- Revision:
- 11:91d827c62a50
- Parent:
- 9:cc60d28643cb
- Child:
- 12:8604a4c1c708
Enkele filter, werkend, variabelen naar '1' genoemd
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
wiesdat | 0:bed29da02e8b | 1 | #include "mbed.h" |
wiesdat | 0:bed29da02e8b | 2 | #include "MODSERIAL.h" |
wiesdat | 0:bed29da02e8b | 3 | #include "HIDScope.h" |
wiesdat | 5:9415e5be8235 | 4 | #include <iostream> |
wiesdat | 0:bed29da02e8b | 5 | |
wiesdat | 6:6f0bc2e465b0 | 6 | #define A1LP1 0.018180963222803 |
wiesdat | 6:6f0bc2e465b0 | 7 | #define A0LP1 0.016544013176248 |
wiesdat | 6:6f0bc2e465b0 | 8 | #define B1LP1 -1.718913340044714 |
wiesdat | 6:6f0bc2e465b0 | 9 | #define B0LP1 0.753638316443765 |
wiesdat | 2:620ff9f02d62 | 10 | |
wiesdat | 6:6f0bc2e465b0 | 11 | #define A1HP1 -1.999801878951505 |
wiesdat | 6:6f0bc2e465b0 | 12 | #define A0HP1 0.999801878951505 |
wiesdat | 6:6f0bc2e465b0 | 13 | #define B1HP1 -1.971717601075000 |
wiesdat | 6:6f0bc2e465b0 | 14 | #define B0HP1 0.972111984032897 |
wiesdat | 6:6f0bc2e465b0 | 15 | |
wiesdat | 6:6f0bc2e465b0 | 16 | #define A0N 0.99436777112 |
wiesdat | 6:6f0bc2e465b0 | 17 | #define A1N -1.89139989664 |
wiesdat | 6:6f0bc2e465b0 | 18 | #define A2N 0.99436777112 |
wiesdat | 6:6f0bc2e465b0 | 19 | #define B1N 1.89070035439 |
wiesdat | 6:6f0bc2e465b0 | 20 | #define B2N -0.988036 |
wiesdat | 1:bc2db3bff4bb | 21 | |
wiesdat | 5:9415e5be8235 | 22 | #define TSAMP 0.001 |
wiesdat | 0:bed29da02e8b | 23 | |
wiesdat | 11:91d827c62a50 | 24 | AnalogIn emg1(PTB1); |
wiesdat | 0:bed29da02e8b | 25 | MODSERIAL pc(USBTX,USBRX); |
wiesdat | 2:620ff9f02d62 | 26 | HIDScope scope(5); |
wiesdat | 0:bed29da02e8b | 27 | Ticker timer; |
wiesdat | 11:91d827c62a50 | 28 | float emg_value1, ylp1, yhp1, yn1; |
wiesdat | 1:bc2db3bff4bb | 29 | volatile bool looptimerflag; |
wiesdat | 11:91d827c62a50 | 30 | float ysum1 = 0, yave1=0 ; |
wiesdat | 0:bed29da02e8b | 31 | |
wiesdat | 5:9415e5be8235 | 32 | |
wiesdat | 5:9415e5be8235 | 33 | float readEMG() |
wiesdat | 0:bed29da02e8b | 34 | { |
wiesdat | 2:620ff9f02d62 | 35 | |
wiesdat | 11:91d827c62a50 | 36 | emg_value1=emg1.read(); |
wiesdat | 11:91d827c62a50 | 37 | return emg_value1; |
wiesdat | 0:bed29da02e8b | 38 | } |
wiesdat | 0:bed29da02e8b | 39 | |
wiesdat | 11:91d827c62a50 | 40 | float notchfilter1(float ylp1) |
wiesdat | 8:37563d2ec529 | 41 | { |
wiesdat | 11:91d827c62a50 | 42 | static float yn1,x1=0,x2=0,y1=0,y2=0,x; |
wiesdat | 8:37563d2ec529 | 43 | x = ylp1; |
wiesdat | 11:91d827c62a50 | 44 | yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2; |
wiesdat | 8:37563d2ec529 | 45 | x2 = x1; |
wiesdat | 8:37563d2ec529 | 46 | x1 = x; |
wiesdat | 8:37563d2ec529 | 47 | y2 = y1; |
wiesdat | 11:91d827c62a50 | 48 | y1 = yn1; |
wiesdat | 11:91d827c62a50 | 49 | return yn1; |
wiesdat | 8:37563d2ec529 | 50 | } |
wiesdat | 8:37563d2ec529 | 51 | |
wiesdat | 8:37563d2ec529 | 52 | |
wiesdat | 11:91d827c62a50 | 53 | float hpfilter1(float yn1) |
wiesdat | 0:bed29da02e8b | 54 | { |
wiesdat | 6:6f0bc2e465b0 | 55 | static float x1=0,y1=0,x2=0, y2=0,x; |
wiesdat | 11:91d827c62a50 | 56 | x = yn1; |
wiesdat | 8:37563d2ec529 | 57 | yhp1 = x + A1HP1*x1 + A0HP1*x2 - B1HP1*y1 - B0HP1*y2; |
wiesdat | 0:bed29da02e8b | 58 | x2 = x1; |
wiesdat | 6:6f0bc2e465b0 | 59 | x1 = x; |
wiesdat | 0:bed29da02e8b | 60 | y2 = y1; |
wiesdat | 6:6f0bc2e465b0 | 61 | y1 = yhp1; |
wiesdat | 2:620ff9f02d62 | 62 | return yhp1; |
wiesdat | 2:620ff9f02d62 | 63 | } |
wiesdat | 2:620ff9f02d62 | 64 | |
wiesdat | 8:37563d2ec529 | 65 | float lpfilter1(float yhp1) |
wiesdat | 2:620ff9f02d62 | 66 | { |
wiesdat | 6:6f0bc2e465b0 | 67 | static float x1=0,y1=0,x2=0, y2=0,x; |
wiesdat | 8:37563d2ec529 | 68 | x = yhp1; |
wiesdat | 6:6f0bc2e465b0 | 69 | ylp1 = A1LP1*x1-B1LP1*y1+A0LP1*x2-B0LP1*y2; |
wiesdat | 6:6f0bc2e465b0 | 70 | x2 = x1; |
wiesdat | 2:620ff9f02d62 | 71 | x1 = x; |
wiesdat | 6:6f0bc2e465b0 | 72 | y2 = y1; |
wiesdat | 6:6f0bc2e465b0 | 73 | y1 = ylp1; |
wiesdat | 2:620ff9f02d62 | 74 | return ylp1; |
wiesdat | 0:bed29da02e8b | 75 | } |
wiesdat | 0:bed29da02e8b | 76 | |
wiesdat | 6:6f0bc2e465b0 | 77 | |
wiesdat | 0:bed29da02e8b | 78 | void viewer() |
wiesdat | 0:bed29da02e8b | 79 | { |
wiesdat | 11:91d827c62a50 | 80 | scope.set(0,emg_value1); |
wiesdat | 11:91d827c62a50 | 81 | scope.set(1,yn1); |
wiesdat | 9:cc60d28643cb | 82 | scope.set(2,yhp1); |
wiesdat | 9:cc60d28643cb | 83 | scope.set(3,ylp1); |
wiesdat | 11:91d827c62a50 | 84 | scope.set(4,yave1); |
wiesdat | 6:6f0bc2e465b0 | 85 | |
wiesdat | 0:bed29da02e8b | 86 | scope.send(); |
wiesdat | 0:bed29da02e8b | 87 | } |
wiesdat | 0:bed29da02e8b | 88 | |
wiesdat | 1:bc2db3bff4bb | 89 | void setlooptimerflag(void) |
wiesdat | 1:bc2db3bff4bb | 90 | { |
wiesdat | 1:bc2db3bff4bb | 91 | looptimerflag = true; |
wiesdat | 1:bc2db3bff4bb | 92 | } |
wiesdat | 1:bc2db3bff4bb | 93 | |
wiesdat | 0:bed29da02e8b | 94 | int main() |
wiesdat | 0:bed29da02e8b | 95 | { |
wiesdat | 0:bed29da02e8b | 96 | pc.baud(115200); |
wiesdat | 1:bc2db3bff4bb | 97 | timer.attach(setlooptimerflag,TSAMP); |
wiesdat | 5:9415e5be8235 | 98 | int n = 0; |
wiesdat | 0:bed29da02e8b | 99 | while(1) { |
wiesdat | 2:620ff9f02d62 | 100 | |
wiesdat | 2:620ff9f02d62 | 101 | while(!looptimerflag) /* do nothing */; |
wiesdat | 2:620ff9f02d62 | 102 | looptimerflag = false; |
wiesdat | 11:91d827c62a50 | 103 | emg_value1 = readEMG(); |
wiesdat | 11:91d827c62a50 | 104 | yn1 = notchfilter1(emg_value1); |
wiesdat | 11:91d827c62a50 | 105 | yhp1 = hpfilter1(yn1); //function hpfilter |
wiesdat | 8:37563d2ec529 | 106 | ylp1 = lpfilter1(yhp1); //function filter |
wiesdat | 8:37563d2ec529 | 107 | |
wiesdat | 11:91d827c62a50 | 108 | ysum1 = ysum1+sqrt(ylp1*ylp1); |
wiesdat | 2:620ff9f02d62 | 109 | n++; |
wiesdat | 5:9415e5be8235 | 110 | |
wiesdat | 5:9415e5be8235 | 111 | if(n==100) { |
wiesdat | 11:91d827c62a50 | 112 | yave1 = abs(ysum1/100); |
wiesdat | 11:91d827c62a50 | 113 | ysum1 = 0; |
wiesdat | 2:620ff9f02d62 | 114 | n = 0; |
wiesdat | 5:9415e5be8235 | 115 | } |
wiesdat | 2:620ff9f02d62 | 116 | |
wiesdat | 5:9415e5be8235 | 117 | viewer(); //functie hidscope |
wiesdat | 5:9415e5be8235 | 118 | |
wiesdat | 2:620ff9f02d62 | 119 | |
wiesdat | 1:bc2db3bff4bb | 120 | } |
wiesdat | 0:bed29da02e8b | 121 | } |