M9 Biorobotics Group 8 / Mbed 2 deprecated EMGfiltering_EMG

Dit is alleen het EMG gedeelte

Dependencies:   mbed HIDScope biquadFilter MODSERIAL FXOS8700Q

main.cpp@5:3d65f89e3755, 2019-10-20 (annotated)

Committer:
Jellehierck
Date:
Sun Oct 20 19:40:57 2019 +0000
Revision:
5:3d65f89e3755
Parent:
4:09a01d2db8f7
Child:
6:5437cc97e1e6
EMG envelope complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IsaRobin 0:6972d0e91af1 1 //c++ script for filtering of measured EMG signals
IsaRobin 0:6972d0e91af1 2 #include "mbed.h" //Base library
IsaRobin 0:6972d0e91af1 3 #include "HIDScope.h" // to see if program is working and EMG is filtered properly
Jellehierck 2:d3e9788ab1b3 4 // #include "QEI.h"// is needed for the encoder
Jellehierck 2:d3e9788ab1b3 5 // #include "MODSERIAL.h"// in order for connection with the pc
Jellehierck 2:d3e9788ab1b3 6 #include "BiQuad.h"
Jellehierck 2:d3e9788ab1b3 7 // #include "FastPWM.h"
Jellehierck 2:d3e9788ab1b3 8 // #include "Arduino.h" //misschien handig omdat we het EMG arduino board gebruiken (?)
Jellehierck 2:d3e9788ab1b3 9 // #include "EMGFilters.h"
IsaRobin 0:6972d0e91af1 10 #include <vector> // For easy array management
IsaRobin 0:6972d0e91af1 11
Jellehierck 2:d3e9788ab1b3 12 // PC serial connection
Jellehierck 4:09a01d2db8f7 13 HIDScope scope( 2 );
Jellehierck 2:d3e9788ab1b3 14 // MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 15
IsaRobin 0:6972d0e91af1 16 //EMG inputs definieren
IsaRobin 0:6972d0e91af1 17 AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting
IsaRobin 0:6972d0e91af1 18 AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting
IsaRobin 0:6972d0e91af1 19 AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting
IsaRobin 0:6972d0e91af1 20
Jellehierck 4:09a01d2db8f7 21 // LED
Jellehierck 4:09a01d2db8f7 22 DigitalOut led(LED_GREEN);
Jellehierck 4:09a01d2db8f7 23
IsaRobin 0:6972d0e91af1 24 //variablen voor EMG
IsaRobin 0:6972d0e91af1 25 double emg1;
IsaRobin 0:6972d0e91af1 26 double emg2;
IsaRobin 0:6972d0e91af1 27 double emg3;
IsaRobin 0:6972d0e91af1 28 double notch1;
IsaRobin 0:6972d0e91af1 29 double notch2;
IsaRobin 0:6972d0e91af1 30 double notch3;
IsaRobin 0:6972d0e91af1 31 double highpass1;
IsaRobin 0:6972d0e91af1 32 double highpass2;
IsaRobin 0:6972d0e91af1 33 double highpass3;
IsaRobin 0:6972d0e91af1 34 double lowpass1;
IsaRobin 0:6972d0e91af1 35 double lowpass2;
IsaRobin 0:6972d0e91af1 36 double lowpass3;
IsaRobin 0:6972d0e91af1 37 double rectify1;
IsaRobin 0:6972d0e91af1 38 double rectify2;
IsaRobin 0:6972d0e91af1 39 double rectify3;
IsaRobin 0:6972d0e91af1 40
Jellehierck 4:09a01d2db8f7 41 // Initialize tickers
Jellehierck 4:09a01d2db8f7 42 Ticker tickSample;
Jellehierck 4:09a01d2db8f7 43
Jellehierck 4:09a01d2db8f7 44 // Sample rate
Jellehierck 4:09a01d2db8f7 45 const double Fs = 500; //Hz
Jellehierck 4:09a01d2db8f7 46
Jellehierck 3:c0ece64850db 47 // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 48 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 49 BiQuad bq_notch(0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882);
Jellehierck 1:059cca298369 50
Jellehierck 3:c0ece64850db 51 // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 52 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 53 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 54 BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025);
Jellehierck 3:c0ece64850db 55 BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078);
Jellehierck 4:09a01d2db8f7 56 BiQuadChain bqc_notch_high; // Used to chain two 2nd other filters into a 4th order filter
IsaRobin 0:6972d0e91af1 57
Jellehierck 3:c0ece64850db 58 // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 59 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 60 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 61 BiQuad bq_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138);
Jellehierck 3:c0ece64850db 62 BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162);
Jellehierck 3:c0ece64850db 63 BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter
Jellehierck 2:d3e9788ab1b3 64
Jellehierck 2:d3e9788ab1b3 65 void sample()
Jellehierck 2:d3e9788ab1b3 66 {
Jellehierck 4:09a01d2db8f7 67 // Read EMG inputs
Jellehierck 2:d3e9788ab1b3 68 emg1 = emg1_in.read();
Jellehierck 2:d3e9788ab1b3 69 emg2 = emg2_in.read();
Jellehierck 2:d3e9788ab1b3 70 emg3 = emg3_in.read();
Jellehierck 4:09a01d2db8f7 71
Jellehierck 4:09a01d2db8f7 72 // Output raw EMG input
Jellehierck 4:09a01d2db8f7 73 scope.set(0, emg1 );
Jellehierck 5:3d65f89e3755 74
Jellehierck 5:3d65f89e3755 75 // Filter notch and highpass
Jellehierck 5:3d65f89e3755 76 double emg1_n_hp = bqc_notch_high.step( emg1 );
Jellehierck 5:3d65f89e3755 77
Jellehierck 5:3d65f89e3755 78 // Rectify
Jellehierck 5:3d65f89e3755 79 double emg1_rectify = fabs( emg1_n_hp );
Jellehierck 5:3d65f89e3755 80
Jellehierck 5:3d65f89e3755 81 // Filter lowpass (completes envelope)
Jellehierck 5:3d65f89e3755 82 double emg1_env = bqc_low.step( emg1_rectify );
Jellehierck 4:09a01d2db8f7 83
Jellehierck 4:09a01d2db8f7 84 // Output EMG after filters
Jellehierck 5:3d65f89e3755 85 scope.set(1, emg1_env );
Jellehierck 4:09a01d2db8f7 86 scope.send();
Jellehierck 2:d3e9788ab1b3 87 }
IsaRobin 0:6972d0e91af1 88
Jellehierck 4:09a01d2db8f7 89 /*
IsaRobin 0:6972d0e91af1 90 //notch filter toepassen
IsaRobin 0:6972d0e91af1 91 notch1 = N1.step(emg1);
IsaRobin 0:6972d0e91af1 92 notch2 = N2.step(emg2);
IsaRobin 0:6972d0e91af1 93 notch3 = N3.step(emg3);
IsaRobin 0:6972d0e91af1 94
Jellehierck 2:d3e9788ab1b3 95 //high pass filter
IsaRobin 0:6972d0e91af1 96 high1 = H1.step(notch1);
IsaRobin 0:6972d0e91af1 97 high2 = H2.step(notch2);
IsaRobin 0:6972d0e91af1 98 high3 = H3.step(notch3);
IsaRobin 0:6972d0e91af1 99
IsaRobin 0:6972d0e91af1 100 //rectify toepassen, oftewel absolute waarde van EMG signaal nemen
IsaRobin 0:6972d0e91af1 101 absolute1 = fabs(high1);
IsaRobin 0:6972d0e91af1 102 absolute2 = fabs(high2);
IsaRobin 0:6972d0e91af1 103 absolute3 = fabs(high3);
IsaRobin 0:6972d0e91af1 104
IsaRobin 0:6972d0e91af1 105 //low pass filter
IsaRobin 0:6972d0e91af1 106 low1 = L1.step(absolute1);
IsaRobin 0:6972d0e91af1 107 low2 = L2.step(absolute2);
IsaRobin 0:6972d0e91af1 108 low3 = L3.step(absolute3);
Jellehierck 4:09a01d2db8f7 109 */
Jellehierck 4:09a01d2db8f7 110
Jellehierck 5:3d65f89e3755 111 void main()
Jellehierck 4:09a01d2db8f7 112 {
Jellehierck 4:09a01d2db8f7 113 const double Ts = 1/Fs;
Jellehierck 4:09a01d2db8f7 114 tickSample.attach(&sample, Ts);
Jellehierck 5:3d65f89e3755 115
Jellehierck 5:3d65f89e3755 116 bqc_notch_high.add( &bq_notch ).add( &bq_H1 ).add( &bq_H2 );
Jellehierck 5:3d65f89e3755 117 bqc_low.add( &bq_L1 ).add( &bq_L2 );
Jellehierck 4:09a01d2db8f7 118
Jellehierck 4:09a01d2db8f7 119 while(true) {
Jellehierck 4:09a01d2db8f7 120 led = !led;
Jellehierck 4:09a01d2db8f7 121 wait(0.5);
Jellehierck 4:09a01d2db8f7 122 }
Jellehierck 4:09a01d2db8f7 123 }