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@0:5c4ee2c81f02, 2017-10-12 (annotated)

Committer:
JornJan
Date:
Thu Oct 12 10:19:08 2017 +0000
Revision:
0:5c4ee2c81f02
Child:
2:7e0279519cbf
12-okt;

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 0:5c4ee2c81f02 13
JornJan 0:5c4ee2c81f02 14 //Benoemen van de variabelen die in de VOID's gebruikt gaan worden
JornJan 0:5c4ee2c81f02 15 double emga = emg0.read(); //EMG1
JornJan 0:5c4ee2c81f02 16 double emgb = emg1.read(); //EMG2
JornJan 0:5c4ee2c81f02 17
JornJan 0:5c4ee2c81f02 18 //Aanmaken filter variabelen
JornJan 0:5c4ee2c81f02 19 float ah[3]={1, 0, 0.1716};
JornJan 0:5c4ee2c81f02 20 float bh[3]={0.2929, -0.5858, 0.2929};
JornJan 0:5c4ee2c81f02 21
JornJan 0:5c4ee2c81f02 22 //innitial conditions high pass filter
JornJan 0:5c4ee2c81f02 23 float emg_hpf[3]={0, 0, 0};
JornJan 0:5c4ee2c81f02 24 float emg_in[3]={0, 0, 0};
JornJan 0:5c4ee2c81f02 25
JornJan 0:5c4ee2c81f02 26 // coëfficienten high pass filter
JornJan 0:5c4ee2c81f02 27 float al[3]={1, -1.7347, 0.7660};
JornJan 0:5c4ee2c81f02 28 float bl[3]={0.0078, 0.0156, 0.0078};
JornJan 0:5c4ee2c81f02 29
JornJan 0:5c4ee2c81f02 30 //innitial conditions low pass filter
JornJan 0:5c4ee2c81f02 31 float emg_lpf[3]={0, 0, 0};
JornJan 0:5c4ee2c81f02 32 float emg_abs[3]={0, 0, 0};
JornJan 0:5c4ee2c81f02 33
JornJan 0:5c4ee2c81f02 34 //Aanmaken van de verschillende tickers
JornJan 0:5c4ee2c81f02 35 Ticker tick_sample;
JornJan 0:5c4ee2c81f02 36
JornJan 0:5c4ee2c81f02 37 void aansturing()
JornJan 0:5c4ee2c81f02 38 {
JornJan 0:5c4ee2c81f02 39 timer.reset();
JornJan 0:5c4ee2c81f02 40 timer.start();
JornJan 0:5c4ee2c81f02 41
JornJan 0:5c4ee2c81f02 42 emga = emg0.read();
JornJan 0:5c4ee2c81f02 43 emgb = emg1.read();
JornJan 0:5c4ee2c81f02 44 emg_in[0]=emga;
JornJan 0:5c4ee2c81f02 45
JornJan 0:5c4ee2c81f02 46 //Filter
JornJan 0:5c4ee2c81f02 47 // high pass filter
JornJan 0:5c4ee2c81f02 48 emg_hpf[0]=bh[0]*emg_in[0] +bh[1]*emg_in[1] +bh[2]*emg_in[2] -ah[1]*emg_hpf[1] -ah[2]*emg_hpf[2];
JornJan 0:5c4ee2c81f02 49
JornJan 0:5c4ee2c81f02 50 emg_in[2]=emg_in[1];
JornJan 0:5c4ee2c81f02 51 emg_in[1]=emg_in[0];
JornJan 0:5c4ee2c81f02 52 emg_hpf[2]=emg_hpf[1];
JornJan 0:5c4ee2c81f02 53 emg_hpf[2]=emg_hpf[0];
JornJan 0:5c4ee2c81f02 54
JornJan 0:5c4ee2c81f02 55
JornJan 0:5c4ee2c81f02 56 //absolute value
JornJan 0:5c4ee2c81f02 57 emg_abs[0]=fabs(emg_hpf[0]);
JornJan 0:5c4ee2c81f02 58
JornJan 0:5c4ee2c81f02 59
JornJan 0:5c4ee2c81f02 60 //low pass filter
JornJan 0:5c4ee2c81f02 61 emg_lpf[0]=bl[0]*emg_abs[0] +bl[1]*emg_abs[1] +bl[2]*emg_abs[2] -al[1]*emg_lpf[1] -al[2]*emg_lpf[2];
JornJan 0:5c4ee2c81f02 62
JornJan 0:5c4ee2c81f02 63 emg_abs[2]=emg_abs[1];
JornJan 0:5c4ee2c81f02 64 emg_abs[1]=emg_abs[0];
JornJan 0:5c4ee2c81f02 65 emg_lpf[2]=emg_lpf[1];
JornJan 0:5c4ee2c81f02 66 emg_lpf[1]=emg_lpf[0];
JornJan 0:5c4ee2c81f02 67
JornJan 0:5c4ee2c81f02 68 if (emg_lpf[1]>0.05)
JornJan 0:5c4ee2c81f02 69 {
JornJan 0:5c4ee2c81f02 70 motor1MagnitudePin = emg_lpf[1];
JornJan 0:5c4ee2c81f02 71 motor1DirectionPin = 0;
JornJan 0:5c4ee2c81f02 72 }
JornJan 0:5c4ee2c81f02 73 else
JornJan 0:5c4ee2c81f02 74 {
JornJan 0:5c4ee2c81f02 75 motor1MagnitudePin = 0;
JornJan 0:5c4ee2c81f02 76 motor1DirectionPin = 0;
JornJan 0:5c4ee2c81f02 77 }
JornJan 0:5c4ee2c81f02 78 scope.set(0, emg0.read());
JornJan 0:5c4ee2c81f02 79 scope.send();
JornJan 0:5c4ee2c81f02 80 scope.set(1, emg_lpf[1]);
JornJan 0:5c4ee2c81f02 81 scope.send();
JornJan 0:5c4ee2c81f02 82
JornJan 0:5c4ee2c81f02 83 timer.stop();
JornJan 0:5c4ee2c81f02 84 pc.printf("time taken was %f milliseconds\n\r", timer.read_ms());
JornJan 0:5c4ee2c81f02 85
JornJan 0:5c4ee2c81f02 86 }
JornJan 0:5c4ee2c81f02 87
JornJan 0:5c4ee2c81f02 88
JornJan 0:5c4ee2c81f02 89 int main()
JornJan 0:5c4ee2c81f02 90 {
JornJan 0:5c4ee2c81f02 91 //Deze tickers roepen de verschillende voids aan
JornJan 0:5c4ee2c81f02 92 pc.baud(9600);
JornJan 0:5c4ee2c81f02 93 tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan
JornJan 0:5c4ee2c81f02 94 }