thijs ruikes / Mbed 2 deprecated PROJECT-BUTTFILTER

script zover

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@14:4c33f6240118, 2014-10-22 (annotated)

Committer:
wiesdat
Date:
Wed Oct 22 12:51:15 2014 +0000
Revision:
14:4c33f6240118
Parent:
13:23130c3a37b1
Child:
15:2adb036a191f
v1.02 filters in functie gezet

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