Arthur de Lange / Mbed 2 deprecated Filter_check

12-okt

Dependencies:   HIDScope mbed

Fork of Filter_check by Jorn-Jan van de Beld

main.cpp@4:8ed071e5e3c9, 2017-10-13 (annotated)

Committer:
JornJan
Date:
Fri Oct 13 11:57:15 2017 +0000
Revision:
4:8ed071e5e3c9
Parent:
3:3e5d899a3c8a
Child:
5:41d4aac93351
Filter met timer;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornJan 0:5c4ee2c81f02 1 #include "mbed.h"
JornJan 0:5c4ee2c81f02 2 #include "Serial.h"
JornJan 0:5c4ee2c81f02 3 #include "math.h"
JornJan 0:5c4ee2c81f02 4 #include "HIDScope.h"
JornJan 0:5c4ee2c81f02 5
JornJan 0:5c4ee2c81f02 6 Serial pc(USBTX, USBRX); //Serial PC connectie
JornJan 0:5c4ee2c81f02 7 AnalogIn emg0( A0 ); //EMG1 op A0
JornJan 0:5c4ee2c81f02 8 AnalogIn emg1( A1 ); //EMG2 op A1
JornJan 0:5c4ee2c81f02 9 DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord)
JornJan 0:5c4ee2c81f02 10 PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord)
JornJan 0:5c4ee2c81f02 11 Timer timer; //timer voor duur script
JornJan 0:5c4ee2c81f02 12 HIDScope scope(2); //Maakt de scopes aan
JornJan 4:8ed071e5e3c9 13 int n=0;
JornJan 0:5c4ee2c81f02 14
JornJan 0:5c4ee2c81f02 15 //Benoemen van de variabelen die in de VOID's gebruikt gaan worden
JornJan 0:5c4ee2c81f02 16 double emga = emg0.read(); //EMG1
JornJan 0:5c4ee2c81f02 17 double emgb = emg1.read(); //EMG2
JornJan 0:5c4ee2c81f02 18
JornJan 4:8ed071e5e3c9 19 // coëfficienten notch filter
JornJan 4:8ed071e5e3c9 20 double an[3]={1.0000, -0.0000, 0.9561};
JornJan 4:8ed071e5e3c9 21 double bn[3]={0.9780, -0.0000, 0.9780};
JornJan 4:8ed071e5e3c9 22
JornJan 4:8ed071e5e3c9 23 // innitial conditions notch filter
JornJan 4:8ed071e5e3c9 24 double emg_nf[3]={0,0,0};
JornJan 4:8ed071e5e3c9 25 double emg_in[3]={0, 0, 0};
JornJan 4:8ed071e5e3c9 26
JornJan 4:8ed071e5e3c9 27 //Aanmaken filter coëfficienten
JornJan 4:8ed071e5e3c9 28 double ah[3]={1.0000, -1.9778, 0.9780};
JornJan 4:8ed071e5e3c9 29 double bh[3]={0.9890, -1.9779, 0.9890};
JornJan 0:5c4ee2c81f02 30
JornJan 0:5c4ee2c81f02 31 //innitial conditions high pass filter
arthurdelange 2:7e0279519cbf 32 double emg_hpf[3]={0, 0, 0};
JornJan 0:5c4ee2c81f02 33
JornJan 4:8ed071e5e3c9 34 // coëfficienten low pass filter
JornJan 4:8ed071e5e3c9 35 double al[5]={1.0000, -3.5897, 4.8513, -2.9241, 0.6630};
JornJan 4:8ed071e5e3c9 36 double bl[5]={0.0000312, 0.0001250, 0.0001874, 0.0001250, 0.0000312};
JornJan 0:5c4ee2c81f02 37
JornJan 0:5c4ee2c81f02 38 //innitial conditions low pass filter
JornJan 4:8ed071e5e3c9 39 double emg_lpf[5]={0, 0, 0, 0, 0};
JornJan 4:8ed071e5e3c9 40 double emg_abs[5]={0, 0, 0, 0, 0};
arthurdelange 2:7e0279519cbf 41
arthurdelange 2:7e0279519cbf 42 double emg_lpfg;
JornJan 0:5c4ee2c81f02 43
JornJan 0:5c4ee2c81f02 44 //Aanmaken van de verschillende tickers
JornJan 0:5c4ee2c81f02 45 Ticker tick_sample;
JornJan 0:5c4ee2c81f02 46
JornJan 4:8ed071e5e3c9 47
JornJan 0:5c4ee2c81f02 48 void aansturing()
JornJan 0:5c4ee2c81f02 49 {
JornJan 4:8ed071e5e3c9 50 timer.reset();
JornJan 0:5c4ee2c81f02 51
JornJan 0:5c4ee2c81f02 52 emga = emg0.read();
JornJan 0:5c4ee2c81f02 53 emgb = emg1.read();
arthurdelange 2:7e0279519cbf 54 emg_in[0]=emga-emgb;
JornJan 0:5c4ee2c81f02 55
JornJan 4:8ed071e5e3c9 56 //Filter cascade
JornJan 4:8ed071e5e3c9 57 // notch filter
JornJan 4:8ed071e5e3c9 58 emg_nf[0]=bn[0]*emg_in[0] +bn[1]*emg_in[1] +bn[2]*emg_in[2] -an[1]*emg_nf[1] -an[2]*emg_nf[2];
JornJan 4:8ed071e5e3c9 59
JornJan 0:5c4ee2c81f02 60 // high pass filter
JornJan 4:8ed071e5e3c9 61 emg_hpf[0]=bh[0]*emg_nf[0] +bh[1]*emg_nf[1] +bh[2]*emg_nf[2] -ah[1]*emg_hpf[1] -ah[2]*emg_hpf[2];
JornJan 0:5c4ee2c81f02 62
JornJan 0:5c4ee2c81f02 63 //absolute value
JornJan 0:5c4ee2c81f02 64 emg_abs[0]=fabs(emg_hpf[0]);
JornJan 4:8ed071e5e3c9 65
JornJan 0:5c4ee2c81f02 66 //low pass filter
JornJan 4:8ed071e5e3c9 67 emg_lpf[0]=bl[0]*emg_abs[0] +bl[1]*emg_abs[1] +bl[2]*emg_abs[2] +bl[3]*emg_abs[3] +bl[4]*emg_abs[4] -al[1]*emg_lpf[1] -al[2]*emg_lpf[2] -al[3]*emg_lpf[3] -al[4]*emg_lpf[4];
JornJan 0:5c4ee2c81f02 68
JornJan 4:8ed071e5e3c9 69 // RAM
JornJan 4:8ed071e5e3c9 70 emg_in[2]=emg_in[1];
JornJan 4:8ed071e5e3c9 71 emg_in[1]=emg_in[0];
JornJan 4:8ed071e5e3c9 72
JornJan 4:8ed071e5e3c9 73 emg_nf[2]=emg_nf[1];
JornJan 4:8ed071e5e3c9 74 emg_nf[1]=emg_nf[0];
JornJan 4:8ed071e5e3c9 75
JornJan 4:8ed071e5e3c9 76 emg_hpf[2]=emg_hpf[1];
JornJan 4:8ed071e5e3c9 77 emg_hpf[1]=emg_hpf[0];
JornJan 4:8ed071e5e3c9 78
JornJan 4:8ed071e5e3c9 79 emg_abs[4]=emg_abs[3];
JornJan 4:8ed071e5e3c9 80 emg_abs[3]=emg_abs[2];
JornJan 0:5c4ee2c81f02 81 emg_abs[2]=emg_abs[1];
JornJan 0:5c4ee2c81f02 82 emg_abs[1]=emg_abs[0];
JornJan 4:8ed071e5e3c9 83
JornJan 4:8ed071e5e3c9 84 emg_lpf[4]=emg_lpf[3];
JornJan 4:8ed071e5e3c9 85 emg_lpf[3]=emg_lpf[2];
JornJan 0:5c4ee2c81f02 86 emg_lpf[2]=emg_lpf[1];
JornJan 0:5c4ee2c81f02 87 emg_lpf[1]=emg_lpf[0];
JornJan 4:8ed071e5e3c9 88
JornJan 4:8ed071e5e3c9 89
JornJan 4:8ed071e5e3c9 90
JornJan 4:8ed071e5e3c9 91
JornJan 0:5c4ee2c81f02 92
JornJan 4:8ed071e5e3c9 93 if (emg_lpf[0]>0.01)
JornJan 0:5c4ee2c81f02 94 {
JornJan 4:8ed071e5e3c9 95 motor1MagnitudePin = emg_lpf[0];
JornJan 0:5c4ee2c81f02 96 motor1DirectionPin = 0;
JornJan 0:5c4ee2c81f02 97 }
JornJan 0:5c4ee2c81f02 98 else
JornJan 0:5c4ee2c81f02 99 {
JornJan 0:5c4ee2c81f02 100 motor1MagnitudePin = 0;
JornJan 0:5c4ee2c81f02 101 motor1DirectionPin = 0;
JornJan 0:5c4ee2c81f02 102 }
arthurdelange 2:7e0279519cbf 103 scope.set(0, emg_in[0]);
JornJan 4:8ed071e5e3c9 104 scope.set(1, emg_lpf[0]);
JornJan 0:5c4ee2c81f02 105 scope.send();
JornJan 4:8ed071e5e3c9 106
JornJan 4:8ed071e5e3c9 107 if (n==100)
JornJan 4:8ed071e5e3c9 108 {
JornJan 4:8ed071e5e3c9 109 pc.printf("time taken was %d microseconds\n\r", timer.read_us());
JornJan 4:8ed071e5e3c9 110 n=0;
JornJan 4:8ed071e5e3c9 111 }
JornJan 4:8ed071e5e3c9 112 else
JornJan 4:8ed071e5e3c9 113 {
JornJan 4:8ed071e5e3c9 114 n=n+1;
JornJan 4:8ed071e5e3c9 115 }
JornJan 0:5c4ee2c81f02 116
JornJan 4:8ed071e5e3c9 117 }
JornJan 0:5c4ee2c81f02 118
JornJan 0:5c4ee2c81f02 119
JornJan 0:5c4ee2c81f02 120 int main()
JornJan 0:5c4ee2c81f02 121 {
JornJan 0:5c4ee2c81f02 122 //Deze tickers roepen de verschillende voids aan
arthurdelange 2:7e0279519cbf 123 pc.baud(115200);
JornJan 4:8ed071e5e3c9 124 timer.start();
JornJan 0:5c4ee2c81f02 125 tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan
JornJan 0:5c4ee2c81f02 126 }