thijs ruikes / Mbed 2 deprecated PROJECT-BUTTFILTER

script zover

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@15:2adb036a191f, 2014-10-22 (annotated)

Committer:
wiesdat
Date:
Wed Oct 22 13:28:24 2014 +0000
Revision:
15:2adb036a191f
Parent:
14:4c33f6240118
Child:
16:f7881b420c83
v1.03

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