thijs ruikes / Mbed 2 deprecated PROJECT-BUTTFILTER

script zover

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@12:8604a4c1c708, 2014-10-22 (annotated)

Committer:
wiesdat
Date:
Wed Oct 22 10:42:46 2014 +0000
Revision:
12:8604a4c1c708
Parent:
11:91d827c62a50
Child:
13:23130c3a37b1
Beide signalen werkend! v1.0

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 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 12:8604a4c1c708 25 AnalogIn emg2(PTB0);
wiesdat 0:bed29da02e8b 26 MODSERIAL pc(USBTX,USBRX);
wiesdat 2:620ff9f02d62 27 HIDScope scope(5);
wiesdat 0:bed29da02e8b 28 Ticker timer;
wiesdat 12:8604a4c1c708 29 float emg_value2, ylp2, yhp2, yn2;
wiesdat 11:91d827c62a50 30 float emg_value1, ylp1, yhp1, yn1;
wiesdat 1:bc2db3bff4bb 31 volatile bool looptimerflag;
wiesdat 11:91d827c62a50 32 float ysum1 = 0, yave1=0 ;
wiesdat 12:8604a4c1c708 33 float ysum2 = 0, yave2=0 ;
wiesdat 5:9415e5be8235 34
wiesdat 5:9415e5be8235 35 float readEMG()
wiesdat 0:bed29da02e8b 36 {
wiesdat 2:620ff9f02d62 37
wiesdat 11:91d827c62a50 38 emg_value1=emg1.read();
wiesdat 11:91d827c62a50 39 return emg_value1;
wiesdat 0:bed29da02e8b 40 }
wiesdat 0:bed29da02e8b 41
wiesdat 12:8604a4c1c708 42
wiesdat 12:8604a4c1c708 43 float readEMG2()
wiesdat 12:8604a4c1c708 44 {
wiesdat 12:8604a4c1c708 45
wiesdat 12:8604a4c1c708 46 emg_value2=emg2.read();
wiesdat 12:8604a4c1c708 47 return emg_value2;
wiesdat 12:8604a4c1c708 48 }
wiesdat 12:8604a4c1c708 49
wiesdat 12:8604a4c1c708 50 float notchfilter2(float ylp2)
wiesdat 12:8604a4c1c708 51 {
wiesdat 12:8604a4c1c708 52 static float x1=0,x2=0,y1=0,y2=0,x;
wiesdat 12:8604a4c1c708 53 x = ylp2;
wiesdat 12:8604a4c1c708 54 yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 12:8604a4c1c708 55 x2 = x1;
wiesdat 12:8604a4c1c708 56 x1 = x;
wiesdat 12:8604a4c1c708 57 y2 = y1;
wiesdat 12:8604a4c1c708 58 y1 = yn2;
wiesdat 12:8604a4c1c708 59 return yn2;
wiesdat 12:8604a4c1c708 60 }
wiesdat 12:8604a4c1c708 61
wiesdat 11:91d827c62a50 62 float notchfilter1(float ylp1)
wiesdat 8:37563d2ec529 63 {
wiesdat 11:91d827c62a50 64 static float yn1,x1=0,x2=0,y1=0,y2=0,x;
wiesdat 8:37563d2ec529 65 x = ylp1;
wiesdat 11:91d827c62a50 66 yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 8:37563d2ec529 67 x2 = x1;
wiesdat 8:37563d2ec529 68 x1 = x;
wiesdat 8:37563d2ec529 69 y2 = y1;
wiesdat 11:91d827c62a50 70 y1 = yn1;
wiesdat 11:91d827c62a50 71 return yn1;
wiesdat 8:37563d2ec529 72 }
wiesdat 8:37563d2ec529 73
wiesdat 8:37563d2ec529 74
wiesdat 12:8604a4c1c708 75 float hpfilter2(float yn2)
wiesdat 12:8604a4c1c708 76 {
wiesdat 12:8604a4c1c708 77 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 12:8604a4c1c708 78 x = yn2;
wiesdat 12:8604a4c1c708 79 yhp2 = x + A1HP1*x1 + A0HP1*x2 - B1HP1*y1 - B0HP1*y2;
wiesdat 12:8604a4c1c708 80 x2 = x1;
wiesdat 12:8604a4c1c708 81 x1 = x;
wiesdat 12:8604a4c1c708 82 y2 = y1;
wiesdat 12:8604a4c1c708 83 y1 = yhp2;
wiesdat 12:8604a4c1c708 84 return yhp2;
wiesdat 12:8604a4c1c708 85 }
wiesdat 11:91d827c62a50 86 float hpfilter1(float yn1)
wiesdat 0:bed29da02e8b 87 {
wiesdat 6:6f0bc2e465b0 88 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 11:91d827c62a50 89 x = yn1;
wiesdat 8:37563d2ec529 90 yhp1 = x + A1HP1*x1 + A0HP1*x2 - B1HP1*y1 - B0HP1*y2;
wiesdat 0:bed29da02e8b 91 x2 = x1;
wiesdat 6:6f0bc2e465b0 92 x1 = x;
wiesdat 0:bed29da02e8b 93 y2 = y1;
wiesdat 6:6f0bc2e465b0 94 y1 = yhp1;
wiesdat 2:620ff9f02d62 95 return yhp1;
wiesdat 2:620ff9f02d62 96 }
wiesdat 2:620ff9f02d62 97
wiesdat 12:8604a4c1c708 98 float lpfilter2(float yhp2)
wiesdat 12:8604a4c1c708 99 {
wiesdat 12:8604a4c1c708 100 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 12:8604a4c1c708 101 x = yhp2;
wiesdat 12:8604a4c1c708 102 ylp2 = A1LP1*x1-B1LP1*y1+A0LP1*x2-B0LP1*y2;
wiesdat 12:8604a4c1c708 103 x2 = x1;
wiesdat 12:8604a4c1c708 104 x1 = x;
wiesdat 12:8604a4c1c708 105 y2 = y1;
wiesdat 12:8604a4c1c708 106 y1 = ylp2;
wiesdat 12:8604a4c1c708 107 return ylp2;
wiesdat 12:8604a4c1c708 108 }
wiesdat 12:8604a4c1c708 109
wiesdat 8:37563d2ec529 110 float lpfilter1(float yhp1)
wiesdat 2:620ff9f02d62 111 {
wiesdat 6:6f0bc2e465b0 112 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 8:37563d2ec529 113 x = yhp1;
wiesdat 6:6f0bc2e465b0 114 ylp1 = A1LP1*x1-B1LP1*y1+A0LP1*x2-B0LP1*y2;
wiesdat 6:6f0bc2e465b0 115 x2 = x1;
wiesdat 2:620ff9f02d62 116 x1 = x;
wiesdat 6:6f0bc2e465b0 117 y2 = y1;
wiesdat 6:6f0bc2e465b0 118 y1 = ylp1;
wiesdat 2:620ff9f02d62 119 return ylp1;
wiesdat 0:bed29da02e8b 120 }
wiesdat 0:bed29da02e8b 121
wiesdat 6:6f0bc2e465b0 122
wiesdat 12:8604a4c1c708 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 2:620ff9f02d62 149 while(!looptimerflag) /* do nothing */;
wiesdat 2:620ff9f02d62 150 looptimerflag = false;
wiesdat 11:91d827c62a50 151 emg_value1 = readEMG();
wiesdat 11:91d827c62a50 152 yn1 = notchfilter1(emg_value1);
wiesdat 11:91d827c62a50 153 yhp1 = hpfilter1(yn1); //function hpfilter
wiesdat 8:37563d2ec529 154 ylp1 = lpfilter1(yhp1); //function filter
wiesdat 8:37563d2ec529 155
wiesdat 11:91d827c62a50 156 ysum1 = ysum1+sqrt(ylp1*ylp1);
wiesdat 2:620ff9f02d62 157 n++;
wiesdat 5:9415e5be8235 158 if(n==100) {
wiesdat 11:91d827c62a50 159 yave1 = abs(ysum1/100);
wiesdat 11:91d827c62a50 160 ysum1 = 0;
wiesdat 2:620ff9f02d62 161 n = 0;
wiesdat 5:9415e5be8235 162 }
wiesdat 2:620ff9f02d62 163
wiesdat 12:8604a4c1c708 164 emg_value2 = readEMG2();
wiesdat 12:8604a4c1c708 165 yn2 = notchfilter2(emg_value2);
wiesdat 12:8604a4c1c708 166 yhp2 = hpfilter2(yn2); //function hpfilter
wiesdat 12:8604a4c1c708 167 ylp2 = lpfilter2(yhp2); //function filter
wiesdat 12:8604a4c1c708 168
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 5:9415e5be8235 178 viewer(); //functie hidscope
wiesdat 5:9415e5be8235 179
wiesdat 2:620ff9f02d62 180
wiesdat 1:bc2db3bff4bb 181 }
wiesdat 0:bed29da02e8b 182 }