M9 Biorobotics Group 8 / Mbed 2 deprecated EMGfiltering_EMG

Dit is alleen het EMG gedeelte

Dependencies:   mbed HIDScope biquadFilter MODSERIAL FXOS8700Q

main.cpp@7:7a088536f1c9, 2019-10-21 (annotated)

Committer:
Jellehierck
Date:
Mon Oct 21 09:21:17 2019 +0000
Revision:
7:7a088536f1c9
Parent:
6:5437cc97e1e6
Child:
8:ea3de43c9e8b
Working on calibration

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
Jellehierck 7:7a088536f1c9 11 #include <numeric> // For manipulating array data
IsaRobin 0:6972d0e91af1 12
Jellehierck 2:d3e9788ab1b3 13 // PC serial connection
Jellehierck 4:09a01d2db8f7 14 HIDScope scope( 2 );
Jellehierck 2:d3e9788ab1b3 15 // MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 16
IsaRobin 0:6972d0e91af1 17 //EMG inputs definieren
IsaRobin 0:6972d0e91af1 18 AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting
IsaRobin 0:6972d0e91af1 19 AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting
IsaRobin 0:6972d0e91af1 20 AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting
IsaRobin 0:6972d0e91af1 21
Jellehierck 4:09a01d2db8f7 22 // LED
Jellehierck 6:5437cc97e1e6 23 DigitalOut led_g(LED_GREEN);
Jellehierck 6:5437cc97e1e6 24 DigitalOut led_r(LED_RED);
Jellehierck 4:09a01d2db8f7 25
IsaRobin 0:6972d0e91af1 26 //variablen voor EMG
IsaRobin 0:6972d0e91af1 27 double emg1;
IsaRobin 0:6972d0e91af1 28 double emg2;
IsaRobin 0:6972d0e91af1 29 double emg3;
Jellehierck 7:7a088536f1c9 30
Jellehierck 7:7a088536f1c9 31 vector<double> emg1_cal;
Jellehierck 7:7a088536f1c9 32
Jellehierck 7:7a088536f1c9 33 int i_cal;
IsaRobin 0:6972d0e91af1 34
Jellehierck 4:09a01d2db8f7 35 // Initialize tickers
Jellehierck 4:09a01d2db8f7 36 Ticker tickSample;
Jellehierck 7:7a088536f1c9 37 Timeout timeoutCalibrationMVC;
Jellehierck 7:7a088536f1c9 38 Ticker tickSampleCalibration;
Jellehierck 4:09a01d2db8f7 39
Jellehierck 4:09a01d2db8f7 40 // Sample rate
Jellehierck 4:09a01d2db8f7 41 const double Fs = 500; //Hz
Jellehierck 7:7a088536f1c9 42 const double Ts = 1/Fs; //sec
Jellehierck 4:09a01d2db8f7 43
Jellehierck 3:c0ece64850db 44 // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 45 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 46 BiQuad bq_notch(0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882);
Jellehierck 1:059cca298369 47
Jellehierck 3:c0ece64850db 48 // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 49 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 50 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 51 BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025);
Jellehierck 3:c0ece64850db 52 BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078);
Jellehierck 4:09a01d2db8f7 53 BiQuadChain bqc_notch_high; // Used to chain two 2nd other filters into a 4th order filter
IsaRobin 0:6972d0e91af1 54
Jellehierck 3:c0ece64850db 55 // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 56 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 57 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 58 BiQuad bq_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138);
Jellehierck 3:c0ece64850db 59 BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162);
Jellehierck 3:c0ece64850db 60 BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter
Jellehierck 2:d3e9788ab1b3 61
Jellehierck 7:7a088536f1c9 62 double stdev(const vector<double> &vect)
Jellehierck 7:7a088536f1c9 63 {
Jellehierck 7:7a088536f1c9 64
Jellehierck 7:7a088536f1c9 65 }
Jellehierck 7:7a088536f1c9 66
Jellehierck 6:5437cc97e1e6 67 // Check if filters are stable
Jellehierck 6:5437cc97e1e6 68 bool checkBQChainStable()
Jellehierck 6:5437cc97e1e6 69 {
Jellehierck 6:5437cc97e1e6 70 bool n_hp_stable = bqc_notch_high.stable();
Jellehierck 6:5437cc97e1e6 71 bool l_stable = bqc_low.stable();
Jellehierck 6:5437cc97e1e6 72
Jellehierck 6:5437cc97e1e6 73 if (n_hp_stable && l_stable) {
Jellehierck 6:5437cc97e1e6 74 return true;
Jellehierck 6:5437cc97e1e6 75 } else {
Jellehierck 6:5437cc97e1e6 76 return false;
Jellehierck 6:5437cc97e1e6 77 }
Jellehierck 6:5437cc97e1e6 78 }
Jellehierck 6:5437cc97e1e6 79
Jellehierck 6:5437cc97e1e6 80
Jellehierck 6:5437cc97e1e6 81 // Read samples, filter samples and output to HIDScope
Jellehierck 2:d3e9788ab1b3 82 void sample()
Jellehierck 2:d3e9788ab1b3 83 {
Jellehierck 4:09a01d2db8f7 84 // Read EMG inputs
Jellehierck 2:d3e9788ab1b3 85 emg1 = emg1_in.read();
Jellehierck 2:d3e9788ab1b3 86 emg2 = emg2_in.read();
Jellehierck 2:d3e9788ab1b3 87 emg3 = emg3_in.read();
Jellehierck 4:09a01d2db8f7 88
Jellehierck 4:09a01d2db8f7 89 // Output raw EMG input
Jellehierck 4:09a01d2db8f7 90 scope.set(0, emg1 );
Jellehierck 6:5437cc97e1e6 91
Jellehierck 5:3d65f89e3755 92 // Filter notch and highpass
Jellehierck 5:3d65f89e3755 93 double emg1_n_hp = bqc_notch_high.step( emg1 );
Jellehierck 6:5437cc97e1e6 94
Jellehierck 5:3d65f89e3755 95 // Rectify
Jellehierck 5:3d65f89e3755 96 double emg1_rectify = fabs( emg1_n_hp );
Jellehierck 6:5437cc97e1e6 97
Jellehierck 5:3d65f89e3755 98 // Filter lowpass (completes envelope)
Jellehierck 5:3d65f89e3755 99 double emg1_env = bqc_low.step( emg1_rectify );
Jellehierck 4:09a01d2db8f7 100
Jellehierck 4:09a01d2db8f7 101 // Output EMG after filters
Jellehierck 5:3d65f89e3755 102 scope.set(1, emg1_env );
Jellehierck 4:09a01d2db8f7 103 scope.send();
Jellehierck 7:7a088536f1c9 104
Jellehierck 7:7a088536f1c9 105
Jellehierck 2:d3e9788ab1b3 106 }
IsaRobin 0:6972d0e91af1 107
Jellehierck 7:7a088536f1c9 108 void sampleCalibration()
Jellehierck 7:7a088536f1c9 109 {
Jellehierck 7:7a088536f1c9 110 // Read EMG inputs
Jellehierck 7:7a088536f1c9 111 emg1 = emg1_in.read();
Jellehierck 7:7a088536f1c9 112 emg2 = emg2_in.read();
Jellehierck 7:7a088536f1c9 113 emg3 = emg3_in.read();
Jellehierck 7:7a088536f1c9 114
Jellehierck 7:7a088536f1c9 115 // Output raw EMG input
Jellehierck 7:7a088536f1c9 116 scope.set(0, emg1 );
Jellehierck 7:7a088536f1c9 117
Jellehierck 7:7a088536f1c9 118 double emg1_n_hp = bqc_notch_high.step( emg1 ); // Filter notch and highpass
Jellehierck 7:7a088536f1c9 119 double emg1_rectify = fabs( emg1_n_hp ); // Filter lowpass (completes envelope)
Jellehierck 7:7a088536f1c9 120 double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
Jellehierck 7:7a088536f1c9 121
Jellehierck 7:7a088536f1c9 122 // Output EMG after filters
Jellehierck 7:7a088536f1c9 123 scope.set(1, emg1_env );
Jellehierck 7:7a088536f1c9 124 scope.send();
Jellehierck 7:7a088536f1c9 125
Jellehierck 7:7a088536f1c9 126 emg1_cal.push_back(emg1_env);
Jellehierck 7:7a088536f1c9 127 }
Jellehierck 7:7a088536f1c9 128
Jellehierck 7:7a088536f1c9 129 void calibrationMVCFinished()
Jellehierck 7:7a088536f1c9 130 {
Jellehierck 7:7a088536f1c9 131 tickSampleCalibration.detach();
Jellehierck 7:7a088536f1c9 132
Jellehierck 7:7a088536f1c9 133 }
Jellehierck 7:7a088536f1c9 134
Jellehierck 7:7a088536f1c9 135 void calibrationMVC()
Jellehierck 7:7a088536f1c9 136 {
Jellehierck 7:7a088536f1c9 137 timeoutCalibrationMVC.attach( &calibrationMVCFinished, 10.0f);
Jellehierck 7:7a088536f1c9 138 tickSampleCalibration.attach( &sampleCalibration, Ts );
Jellehierck 7:7a088536f1c9 139 }
Jellehierck 7:7a088536f1c9 140
Jellehierck 7:7a088536f1c9 141
Jellehierck 7:7a088536f1c9 142
Jellehierck 5:3d65f89e3755 143 void main()
Jellehierck 4:09a01d2db8f7 144 {
Jellehierck 6:5437cc97e1e6 145 // Initialize sample ticker
Jellehierck 4:09a01d2db8f7 146 tickSample.attach(&sample, Ts);
Jellehierck 6:5437cc97e1e6 147
Jellehierck 6:5437cc97e1e6 148 // Create BQ chains to reduce computations
Jellehierck 5:3d65f89e3755 149 bqc_notch_high.add( &bq_notch ).add( &bq_H1 ).add( &bq_H2 );
Jellehierck 5:3d65f89e3755 150 bqc_low.add( &bq_L1 ).add( &bq_L2 );
Jellehierck 4:09a01d2db8f7 151
Jellehierck 6:5437cc97e1e6 152 // If any filter chain is unstable, red led will light up
Jellehierck 6:5437cc97e1e6 153 if (checkBQChainStable) {
Jellehierck 6:5437cc97e1e6 154 led_r = 1; // LED off
Jellehierck 6:5437cc97e1e6 155 } else {
Jellehierck 6:5437cc97e1e6 156 led_r = 0; // LED on
Jellehierck 6:5437cc97e1e6 157 }
Jellehierck 6:5437cc97e1e6 158
Jellehierck 4:09a01d2db8f7 159 while(true) {
Jellehierck 7:7a088536f1c9 160
Jellehierck 6:5437cc97e1e6 161 // Show that system is running
Jellehierck 6:5437cc97e1e6 162 led_g = !led_g;
Jellehierck 4:09a01d2db8f7 163 wait(0.5);
Jellehierck 4:09a01d2db8f7 164 }
Jellehierck 4:09a01d2db8f7 165 }