script zover

Dependencies:   HIDScope MODSERIAL mbed

Committer:
wiesdat
Date:
Wed Oct 22 09:24:13 2014 +0000
Revision:
11:91d827c62a50
Parent:
9:cc60d28643cb
Child:
12:8604a4c1c708
Enkele filter, werkend, variabelen naar '1' genoemd

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 0:bed29da02e8b 25 MODSERIAL pc(USBTX,USBRX);
wiesdat 2:620ff9f02d62 26 HIDScope scope(5);
wiesdat 0:bed29da02e8b 27 Ticker timer;
wiesdat 11:91d827c62a50 28 float emg_value1, ylp1, yhp1, yn1;
wiesdat 1:bc2db3bff4bb 29 volatile bool looptimerflag;
wiesdat 11:91d827c62a50 30 float ysum1 = 0, yave1=0 ;
wiesdat 0:bed29da02e8b 31
wiesdat 5:9415e5be8235 32
wiesdat 5:9415e5be8235 33 float readEMG()
wiesdat 0:bed29da02e8b 34 {
wiesdat 2:620ff9f02d62 35
wiesdat 11:91d827c62a50 36 emg_value1=emg1.read();
wiesdat 11:91d827c62a50 37 return emg_value1;
wiesdat 0:bed29da02e8b 38 }
wiesdat 0:bed29da02e8b 39
wiesdat 11:91d827c62a50 40 float notchfilter1(float ylp1)
wiesdat 8:37563d2ec529 41 {
wiesdat 11:91d827c62a50 42 static float yn1,x1=0,x2=0,y1=0,y2=0,x;
wiesdat 8:37563d2ec529 43 x = ylp1;
wiesdat 11:91d827c62a50 44 yn1 = 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 11:91d827c62a50 48 y1 = yn1;
wiesdat 11:91d827c62a50 49 return yn1;
wiesdat 8:37563d2ec529 50 }
wiesdat 8:37563d2ec529 51
wiesdat 8:37563d2ec529 52
wiesdat 11:91d827c62a50 53 float hpfilter1(float yn1)
wiesdat 0:bed29da02e8b 54 {
wiesdat 6:6f0bc2e465b0 55 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 11:91d827c62a50 56 x = yn1;
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 11:91d827c62a50 80 scope.set(0,emg_value1);
wiesdat 11:91d827c62a50 81 scope.set(1,yn1);
wiesdat 9:cc60d28643cb 82 scope.set(2,yhp1);
wiesdat 9:cc60d28643cb 83 scope.set(3,ylp1);
wiesdat 11:91d827c62a50 84 scope.set(4,yave1);
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 11:91d827c62a50 103 emg_value1 = readEMG();
wiesdat 11:91d827c62a50 104 yn1 = notchfilter1(emg_value1);
wiesdat 11:91d827c62a50 105 yhp1 = hpfilter1(yn1); //function hpfilter
wiesdat 8:37563d2ec529 106 ylp1 = lpfilter1(yhp1); //function filter
wiesdat 8:37563d2ec529 107
wiesdat 11:91d827c62a50 108 ysum1 = ysum1+sqrt(ylp1*ylp1);
wiesdat 2:620ff9f02d62 109 n++;
wiesdat 5:9415e5be8235 110
wiesdat 5:9415e5be8235 111 if(n==100) {
wiesdat 11:91d827c62a50 112 yave1 = abs(ysum1/100);
wiesdat 11:91d827c62a50 113 ysum1 = 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 }