thijs ruikes / Mbed 2 deprecated PROJECT-BUTTFILTER

script zover

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@13:23130c3a37b1, 2014-10-22 (annotated)

Committer:
wiesdat
Date:
Wed Oct 22 10:46:51 2014 +0000
Revision:
13:23130c3a37b1
Parent:
12:8604a4c1c708
Child:
14:4c33f6240118
netter gemaakt v1.01

Who changed what in which revision?

UserRevisionLine numberNew 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 13:23130c3a37b1 6 #define A1LP 0.018180963222803
wiesdat 13:23130c3a37b1 7 #define A0LP 0.016544013176248
wiesdat 13:23130c3a37b1 8 #define B1LP -1.718913340044714
wiesdat 13:23130c3a37b1 9 #define B0LP 0.753638316443765
wiesdat 2:620ff9f02d62 10
wiesdat 13:23130c3a37b1 11 #define A1HP -1.999801878951505
wiesdat 13:23130c3a37b1 12 #define A0HP 0.999801878951505
wiesdat 13:23130c3a37b1 13 #define B1HP -1.971717601075000
wiesdat 13:23130c3a37b1 14 #define B0HP 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 12:8604a4c1c708 25 AnalogIn emg2(PTB0);
wiesdat 13:23130c3a37b1 26
wiesdat 0:bed29da02e8b 27 MODSERIAL pc(USBTX,USBRX);
wiesdat 2:620ff9f02d62 28 HIDScope scope(5);
wiesdat 0:bed29da02e8b 29 Ticker timer;
wiesdat 13:23130c3a37b1 30 volatile bool looptimerflag;
wiesdat 13:23130c3a37b1 31
wiesdat 12:8604a4c1c708 32 float emg_value2, ylp2, yhp2, yn2;
wiesdat 11:91d827c62a50 33 float emg_value1, ylp1, yhp1, yn1;
wiesdat 13:23130c3a37b1 34
wiesdat 11:91d827c62a50 35 float ysum1 = 0, yave1=0 ;
wiesdat 12:8604a4c1c708 36 float ysum2 = 0, yave2=0 ;
wiesdat 5:9415e5be8235 37
wiesdat 13:23130c3a37b1 38 float readEMG1()
wiesdat 0:bed29da02e8b 39 {
wiesdat 2:620ff9f02d62 40
wiesdat 11:91d827c62a50 41 emg_value1=emg1.read();
wiesdat 11:91d827c62a50 42 return emg_value1;
wiesdat 0:bed29da02e8b 43 }
wiesdat 0:bed29da02e8b 44
wiesdat 12:8604a4c1c708 45
wiesdat 12:8604a4c1c708 46 float readEMG2()
wiesdat 12:8604a4c1c708 47 {
wiesdat 12:8604a4c1c708 48
wiesdat 12:8604a4c1c708 49 emg_value2=emg2.read();
wiesdat 12:8604a4c1c708 50 return emg_value2;
wiesdat 12:8604a4c1c708 51 }
wiesdat 12:8604a4c1c708 52
wiesdat 12:8604a4c1c708 53 float notchfilter2(float ylp2)
wiesdat 12:8604a4c1c708 54 {
wiesdat 12:8604a4c1c708 55 static float x1=0,x2=0,y1=0,y2=0,x;
wiesdat 12:8604a4c1c708 56 x = ylp2;
wiesdat 12:8604a4c1c708 57 yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 12:8604a4c1c708 58 x2 = x1;
wiesdat 12:8604a4c1c708 59 x1 = x;
wiesdat 12:8604a4c1c708 60 y2 = y1;
wiesdat 12:8604a4c1c708 61 y1 = yn2;
wiesdat 12:8604a4c1c708 62 return yn2;
wiesdat 12:8604a4c1c708 63 }
wiesdat 12:8604a4c1c708 64
wiesdat 11:91d827c62a50 65 float notchfilter1(float ylp1)
wiesdat 8:37563d2ec529 66 {
wiesdat 11:91d827c62a50 67 static float yn1,x1=0,x2=0,y1=0,y2=0,x;
wiesdat 8:37563d2ec529 68 x = ylp1;
wiesdat 11:91d827c62a50 69 yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 8:37563d2ec529 70 x2 = x1;
wiesdat 8:37563d2ec529 71 x1 = x;
wiesdat 8:37563d2ec529 72 y2 = y1;
wiesdat 11:91d827c62a50 73 y1 = yn1;
wiesdat 11:91d827c62a50 74 return yn1;
wiesdat 8:37563d2ec529 75 }
wiesdat 8:37563d2ec529 76
wiesdat 12:8604a4c1c708 77 float hpfilter2(float yn2)
wiesdat 12:8604a4c1c708 78 {
wiesdat 12:8604a4c1c708 79 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 12:8604a4c1c708 80 x = yn2;
wiesdat 13:23130c3a37b1 81 yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 12:8604a4c1c708 82 x2 = x1;
wiesdat 12:8604a4c1c708 83 x1 = x;
wiesdat 12:8604a4c1c708 84 y2 = y1;
wiesdat 12:8604a4c1c708 85 y1 = yhp2;
wiesdat 12:8604a4c1c708 86 return yhp2;
wiesdat 12:8604a4c1c708 87 }
wiesdat 11:91d827c62a50 88 float hpfilter1(float yn1)
wiesdat 0:bed29da02e8b 89 {
wiesdat 6:6f0bc2e465b0 90 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 11:91d827c62a50 91 x = yn1;
wiesdat 13:23130c3a37b1 92 yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 0:bed29da02e8b 93 x2 = x1;
wiesdat 6:6f0bc2e465b0 94 x1 = x;
wiesdat 0:bed29da02e8b 95 y2 = y1;
wiesdat 6:6f0bc2e465b0 96 y1 = yhp1;
wiesdat 2:620ff9f02d62 97 return yhp1;
wiesdat 2:620ff9f02d62 98 }
wiesdat 2:620ff9f02d62 99
wiesdat 12:8604a4c1c708 100 float lpfilter2(float yhp2)
wiesdat 12:8604a4c1c708 101 {
wiesdat 12:8604a4c1c708 102 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 12:8604a4c1c708 103 x = yhp2;
wiesdat 13:23130c3a37b1 104 ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 12:8604a4c1c708 105 x2 = x1;
wiesdat 12:8604a4c1c708 106 x1 = x;
wiesdat 12:8604a4c1c708 107 y2 = y1;
wiesdat 12:8604a4c1c708 108 y1 = ylp2;
wiesdat 12:8604a4c1c708 109 return ylp2;
wiesdat 12:8604a4c1c708 110 }
wiesdat 12:8604a4c1c708 111
wiesdat 8:37563d2ec529 112 float lpfilter1(float yhp1)
wiesdat 2:620ff9f02d62 113 {
wiesdat 6:6f0bc2e465b0 114 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 8:37563d2ec529 115 x = yhp1;
wiesdat 13:23130c3a37b1 116 ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 6:6f0bc2e465b0 117 x2 = x1;
wiesdat 2:620ff9f02d62 118 x1 = x;
wiesdat 6:6f0bc2e465b0 119 y2 = y1;
wiesdat 6:6f0bc2e465b0 120 y1 = ylp1;
wiesdat 2:620ff9f02d62 121 return ylp1;
wiesdat 0:bed29da02e8b 122 }
wiesdat 0:bed29da02e8b 123
wiesdat 0:bed29da02e8b 124 void viewer()
wiesdat 0:bed29da02e8b 125 {
wiesdat 11:91d827c62a50 126 scope.set(0,emg_value1);
wiesdat 12:8604a4c1c708 127 scope.set(1,ylp1);
wiesdat 12:8604a4c1c708 128 scope.set(2,yave1);
wiesdat 12:8604a4c1c708 129 scope.set(3,ylp2);
wiesdat 12:8604a4c1c708 130 scope.set(4,yave2);
wiesdat 12:8604a4c1c708 131
wiesdat 6:6f0bc2e465b0 132
wiesdat 0:bed29da02e8b 133 scope.send();
wiesdat 0:bed29da02e8b 134 }
wiesdat 0:bed29da02e8b 135
wiesdat 1:bc2db3bff4bb 136 void setlooptimerflag(void)
wiesdat 1:bc2db3bff4bb 137 {
wiesdat 1:bc2db3bff4bb 138 looptimerflag = true;
wiesdat 1:bc2db3bff4bb 139 }
wiesdat 1:bc2db3bff4bb 140
wiesdat 0:bed29da02e8b 141 int main()
wiesdat 0:bed29da02e8b 142 {
wiesdat 0:bed29da02e8b 143 pc.baud(115200);
wiesdat 1:bc2db3bff4bb 144 timer.attach(setlooptimerflag,TSAMP);
wiesdat 5:9415e5be8235 145 int n = 0;
wiesdat 12:8604a4c1c708 146 int m = 0;
wiesdat 0:bed29da02e8b 147 while(1) {
wiesdat 2:620ff9f02d62 148
wiesdat 13:23130c3a37b1 149 while(!looptimerflag);
wiesdat 2:620ff9f02d62 150 looptimerflag = false;
wiesdat 13:23130c3a37b1 151 emg_value1 = readEMG1();
wiesdat 13:23130c3a37b1 152
wiesdat 11:91d827c62a50 153 yn1 = notchfilter1(emg_value1);
wiesdat 13:23130c3a37b1 154 yhp1 = hpfilter1(yn1);
wiesdat 13:23130c3a37b1 155 ylp1 = lpfilter1(yhp1);
wiesdat 11:91d827c62a50 156 ysum1 = ysum1+sqrt(ylp1*ylp1);
wiesdat 2:620ff9f02d62 157 n++;
wiesdat 13:23130c3a37b1 158
wiesdat 5:9415e5be8235 159 if(n==100) {
wiesdat 11:91d827c62a50 160 yave1 = abs(ysum1/100);
wiesdat 11:91d827c62a50 161 ysum1 = 0;
wiesdat 2:620ff9f02d62 162 n = 0;
wiesdat 5:9415e5be8235 163 }
wiesdat 2:620ff9f02d62 164
wiesdat 12:8604a4c1c708 165 emg_value2 = readEMG2();
wiesdat 12:8604a4c1c708 166 yn2 = notchfilter2(emg_value2);
wiesdat 13:23130c3a37b1 167 yhp2 = hpfilter2(yn2);
wiesdat 13:23130c3a37b1 168 ylp2 = lpfilter2(yhp2);
wiesdat 12:8604a4c1c708 169 ysum2 = ysum2 +sqrt(ylp2*ylp2);
wiesdat 12:8604a4c1c708 170 m++;
wiesdat 12:8604a4c1c708 171
wiesdat 12:8604a4c1c708 172 if(m==1000) {
wiesdat 12:8604a4c1c708 173 yave2 = abs(ysum2/1000);
wiesdat 12:8604a4c1c708 174 ysum2 = 0;
wiesdat 12:8604a4c1c708 175 m = 0;
wiesdat 12:8604a4c1c708 176 }
wiesdat 12:8604a4c1c708 177
wiesdat 13:23130c3a37b1 178 viewer();
wiesdat 1:bc2db3bff4bb 179 }
wiesdat 0:bed29da02e8b 180 }