thijs ruikes / Mbed 2 deprecated PROJECT-BUTTFILTER

script zover

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@8:37563d2ec529, 2014-10-20 (annotated)

Committer:
wiesdat
Date:
Mon Oct 20 10:12:00 2014 +0000
Revision:
8:37563d2ec529
Parent:
7:5f1c0bcd3f69
Child:
9:cc60d28643cb
notch eerst, '-' in ylp vervangen;

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 0:bed29da02e8b 24 AnalogIn emg0(PTB1);
wiesdat 0:bed29da02e8b 25 MODSERIAL pc(USBTX,USBRX);
wiesdat 2:620ff9f02d62 26 HIDScope scope(5);
wiesdat 0:bed29da02e8b 27 Ticker timer;
wiesdat 6:6f0bc2e465b0 28 float emg_value, ylp1, yhp1, yn;
wiesdat 1:bc2db3bff4bb 29 volatile bool looptimerflag;
wiesdat 2:620ff9f02d62 30 float ysum = 0, yave=0 ;
wiesdat 0:bed29da02e8b 31
wiesdat 5:9415e5be8235 32
wiesdat 5:9415e5be8235 33 float readEMG()
wiesdat 0:bed29da02e8b 34 {
wiesdat 2:620ff9f02d62 35
wiesdat 5:9415e5be8235 36 emg_value=emg0.read();
wiesdat 5:9415e5be8235 37 return emg_value;
wiesdat 0:bed29da02e8b 38 }
wiesdat 0:bed29da02e8b 39
wiesdat 8:37563d2ec529 40 float notchfilter(float ylp1)
wiesdat 8:37563d2ec529 41 {
wiesdat 8:37563d2ec529 42 static float yn,x1=0,x2=0,y1=0,y2=0,x;
wiesdat 8:37563d2ec529 43 x = ylp1;
wiesdat 8:37563d2ec529 44 yn = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 8:37563d2ec529 45 x2 = x1;
wiesdat 8:37563d2ec529 46 x1 = x;
wiesdat 8:37563d2ec529 47 y2 = y1;
wiesdat 8:37563d2ec529 48 y1 = yn;
wiesdat 8:37563d2ec529 49 return yn;
wiesdat 8:37563d2ec529 50 }
wiesdat 8:37563d2ec529 51
wiesdat 8:37563d2ec529 52
wiesdat 8:37563d2ec529 53 float hpfilter1(float yn)
wiesdat 0:bed29da02e8b 54 {
wiesdat 6:6f0bc2e465b0 55 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 8:37563d2ec529 56 x = yn;
wiesdat 8:37563d2ec529 57 yhp1 = x + A1HP1*x1 + A0HP1*x2 - B1HP1*y1 - B0HP1*y2;
wiesdat 0:bed29da02e8b 58 x2 = x1;
wiesdat 6:6f0bc2e465b0 59 x1 = x;
wiesdat 0:bed29da02e8b 60 y2 = y1;
wiesdat 6:6f0bc2e465b0 61 y1 = yhp1;
wiesdat 2:620ff9f02d62 62 return yhp1;
wiesdat 2:620ff9f02d62 63 }
wiesdat 2:620ff9f02d62 64
wiesdat 8:37563d2ec529 65 float lpfilter1(float yhp1)
wiesdat 2:620ff9f02d62 66 {
wiesdat 6:6f0bc2e465b0 67 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 8:37563d2ec529 68 x = yhp1;
wiesdat 6:6f0bc2e465b0 69 ylp1 = A1LP1*x1-B1LP1*y1+A0LP1*x2-B0LP1*y2;
wiesdat 6:6f0bc2e465b0 70 x2 = x1;
wiesdat 2:620ff9f02d62 71 x1 = x;
wiesdat 6:6f0bc2e465b0 72 y2 = y1;
wiesdat 6:6f0bc2e465b0 73 y1 = ylp1;
wiesdat 2:620ff9f02d62 74 return ylp1;
wiesdat 0:bed29da02e8b 75 }
wiesdat 0:bed29da02e8b 76
wiesdat 6:6f0bc2e465b0 77
wiesdat 0:bed29da02e8b 78 void viewer()
wiesdat 0:bed29da02e8b 79 {
wiesdat 0:bed29da02e8b 80 scope.set(0,emg_value);
wiesdat 5:9415e5be8235 81 scope.set(1,yhp1);
wiesdat 5:9415e5be8235 82 scope.set(2,ylp1);
wiesdat 6:6f0bc2e465b0 83 scope.set(3,yn);
wiesdat 6:6f0bc2e465b0 84 scope.set(4,yave);
wiesdat 6:6f0bc2e465b0 85
wiesdat 0:bed29da02e8b 86 scope.send();
wiesdat 0:bed29da02e8b 87 }
wiesdat 0:bed29da02e8b 88
wiesdat 1:bc2db3bff4bb 89 void setlooptimerflag(void)
wiesdat 1:bc2db3bff4bb 90 {
wiesdat 1:bc2db3bff4bb 91 looptimerflag = true;
wiesdat 1:bc2db3bff4bb 92 }
wiesdat 1:bc2db3bff4bb 93
wiesdat 0:bed29da02e8b 94 int main()
wiesdat 0:bed29da02e8b 95 {
wiesdat 0:bed29da02e8b 96 pc.baud(115200);
wiesdat 1:bc2db3bff4bb 97 timer.attach(setlooptimerflag,TSAMP);
wiesdat 5:9415e5be8235 98 int n = 0;
wiesdat 0:bed29da02e8b 99 while(1) {
wiesdat 2:620ff9f02d62 100
wiesdat 2:620ff9f02d62 101 while(!looptimerflag) /* do nothing */;
wiesdat 2:620ff9f02d62 102 looptimerflag = false;
wiesdat 2:620ff9f02d62 103 emg_value = readEMG();
wiesdat 7:5f1c0bcd3f69 104 yn = notchfilter(emg_value);
wiesdat 8:37563d2ec529 105 yhp1 = hpfilter1(yn); //function hpfilter
wiesdat 8:37563d2ec529 106 ylp1 = lpfilter1(yhp1); //function filter
wiesdat 8:37563d2ec529 107
wiesdat 6:6f0bc2e465b0 108 ysum = ysum+yn;
wiesdat 2:620ff9f02d62 109 n++;
wiesdat 5:9415e5be8235 110
wiesdat 5:9415e5be8235 111 if(n==100) {
wiesdat 5:9415e5be8235 112 yave = ysum/100;
wiesdat 2:620ff9f02d62 113 ysum = 0;
wiesdat 2:620ff9f02d62 114 n = 0;
wiesdat 5:9415e5be8235 115 }
wiesdat 2:620ff9f02d62 116
wiesdat 5:9415e5be8235 117 viewer(); //functie hidscope
wiesdat 5:9415e5be8235 118
wiesdat 2:620ff9f02d62 119
wiesdat 1:bc2db3bff4bb 120 }
wiesdat 0:bed29da02e8b 121 }