thijs ruikes / Mbed 2 deprecated PROJECT-BUTTFILTER

script zover

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@7:5f1c0bcd3f69, 2014-10-20 (annotated)

Committer:
wiesdat
Date:
Mon Oct 20 09:54:15 2014 +0000
Revision:
7:5f1c0bcd3f69
Parent:
6:6f0bc2e465b0
Child:
8:37563d2ec529
Filters apart;

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