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@5:41d4aac93351, 2017-10-13 (annotated)

Committer:
JornJan
Date:
Fri Oct 13 14:24:12 2017 +0000
Revision:
5:41d4aac93351
Parent:
4:8ed071e5e3c9 
13okt stuk (tom);

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 5:41d4aac93351 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 5:41d4aac93351 20 double an[3]={1.0000, -0.0000, 0.9490};
JornJan 5:41d4aac93351 21 double bn[3]={0.9745, -0.0000, 0.9745};
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 5:41d4aac93351 28 double ah[3]={1.0000, -1.9933, 0.9934};
JornJan 5:41d4aac93351 29 double bh[3]={0.9967, -1.9933, 0.9967};
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 5:41d4aac93351 35 double al[5]={1.0000, -3.9179, 5.7571, -3.7603, 0.9212};
JornJan 5:41d4aac93351 36 double bl[5]={0.0585e-6, 0.2338e-6, 0.3507e-6, 0.2338e-6, 0.0585e-6};
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 5:41d4aac93351 52 emga=emg0.read();
JornJan 5:41d4aac93351 53 emg_in[0]=emga;
JornJan 0:5c4ee2c81f02 54
JornJan 4:8ed071e5e3c9 55 //Filter cascade
JornJan 4:8ed071e5e3c9 56 // notch filter
JornJan 4:8ed071e5e3c9 57 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 58
JornJan 0:5c4ee2c81f02 59 // high pass filter
JornJan 4:8ed071e5e3c9 60 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 61
JornJan 0:5c4ee2c81f02 62 //absolute value
JornJan 0:5c4ee2c81f02 63 emg_abs[0]=fabs(emg_hpf[0]);
JornJan 4:8ed071e5e3c9 64
JornJan 0:5c4ee2c81f02 65 //low pass filter
JornJan 4:8ed071e5e3c9 66 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 67
JornJan 4:8ed071e5e3c9 68 // RAM
JornJan 4:8ed071e5e3c9 69 emg_in[2]=emg_in[1];
JornJan 4:8ed071e5e3c9 70 emg_in[1]=emg_in[0];
JornJan 4:8ed071e5e3c9 71
JornJan 4:8ed071e5e3c9 72 emg_nf[2]=emg_nf[1];
JornJan 4:8ed071e5e3c9 73 emg_nf[1]=emg_nf[0];
JornJan 4:8ed071e5e3c9 74
JornJan 4:8ed071e5e3c9 75 emg_hpf[2]=emg_hpf[1];
JornJan 4:8ed071e5e3c9 76 emg_hpf[1]=emg_hpf[0];
JornJan 4:8ed071e5e3c9 77
JornJan 4:8ed071e5e3c9 78 emg_abs[4]=emg_abs[3];
JornJan 4:8ed071e5e3c9 79 emg_abs[3]=emg_abs[2];
JornJan 0:5c4ee2c81f02 80 emg_abs[2]=emg_abs[1];
JornJan 0:5c4ee2c81f02 81 emg_abs[1]=emg_abs[0];
JornJan 4:8ed071e5e3c9 82
JornJan 4:8ed071e5e3c9 83 emg_lpf[4]=emg_lpf[3];
JornJan 4:8ed071e5e3c9 84 emg_lpf[3]=emg_lpf[2];
JornJan 0:5c4ee2c81f02 85 emg_lpf[2]=emg_lpf[1];
JornJan 0:5c4ee2c81f02 86 emg_lpf[1]=emg_lpf[0];
JornJan 4:8ed071e5e3c9 87
JornJan 4:8ed071e5e3c9 88
JornJan 4:8ed071e5e3c9 89
JornJan 4:8ed071e5e3c9 90
JornJan 0:5c4ee2c81f02 91
JornJan 4:8ed071e5e3c9 92 if (emg_lpf[0]>0.01)
JornJan 0:5c4ee2c81f02 93 {
JornJan 4:8ed071e5e3c9 94 motor1MagnitudePin = emg_lpf[0];
JornJan 0:5c4ee2c81f02 95 motor1DirectionPin = 0;
JornJan 0:5c4ee2c81f02 96 }
JornJan 0:5c4ee2c81f02 97 else
JornJan 0:5c4ee2c81f02 98 {
JornJan 0:5c4ee2c81f02 99 motor1MagnitudePin = 0;
JornJan 0:5c4ee2c81f02 100 motor1DirectionPin = 0;
JornJan 0:5c4ee2c81f02 101 }
JornJan 5:41d4aac93351 102 scope.set(0, emg_in[1]);
JornJan 5:41d4aac93351 103 scope.set(1, emg_lpf[1]);
JornJan 0:5c4ee2c81f02 104 scope.send();
JornJan 4:8ed071e5e3c9 105
JornJan 4:8ed071e5e3c9 106 if (n==100)
JornJan 4:8ed071e5e3c9 107 {
JornJan 4:8ed071e5e3c9 108 pc.printf("time taken was %d microseconds\n\r", timer.read_us());
JornJan 4:8ed071e5e3c9 109 n=0;
JornJan 4:8ed071e5e3c9 110 }
JornJan 4:8ed071e5e3c9 111 else
JornJan 4:8ed071e5e3c9 112 {
JornJan 4:8ed071e5e3c9 113 n=n+1;
JornJan 4:8ed071e5e3c9 114 }
JornJan 0:5c4ee2c81f02 115
JornJan 4:8ed071e5e3c9 116 }
JornJan 0:5c4ee2c81f02 117
JornJan 0:5c4ee2c81f02 118
JornJan 0:5c4ee2c81f02 119 int main()
JornJan 0:5c4ee2c81f02 120 {
JornJan 0:5c4ee2c81f02 121 //Deze tickers roepen de verschillende voids aan
arthurdelange 2:7e0279519cbf 122 pc.baud(115200);
JornJan 4:8ed071e5e3c9 123 timer.start();
JornJan 0:5c4ee2c81f02 124 tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan
JornJan 0:5c4ee2c81f02 125 }