thijs ruikes / Mbed 2 deprecated PROJECT-BUTTFILTER

script zover

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@10:c7140e085ff3, 2014-10-20 (annotated)

Committer:
wiesdat
Date:
Mon Oct 20 11:27:55 2014 +0000
Revision:
10:c7140e085ff3
Parent:
9:cc60d28643cb 
Dubbele filter

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