Group 9 BioRobotics / Mbed 2 deprecated EMG2

emg2

Dependencies:   HIDScope biquadFilter mbed QEI

Fork of EMG by Tom Tom

main.cpp@26:1eafb6111ae8, 2018-10-30 (annotated)

Committer:
keeswieriks
Date:
Tue Oct 30 09:20:55 2018 +0000
Revision:
26:1eafb6111ae8
Parent:
25:02f183b944ed
Child:
27:6b4814ef266d
met movavg erbij maar komt nog niet op juiste waarde;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
keeswieriks 21:931fe86dbf5a 3 #include "BiQuad.h"
vsluiter 0:32bb76391d89 4
s1574396 25:02f183b944ed 5 HIDScope scope( 6 );
s1574396 22:bcfee9594007 6 Ticker sample_timer;
s1574396 22:bcfee9594007 7
s1574396 23:dec549767006 8 // Inputs EMG
keeswieriks 21:931fe86dbf5a 9 AnalogIn emg0_in( A0 );
keeswieriks 21:931fe86dbf5a 10 AnalogIn emg1_in( A1 );
keeswieriks 21:931fe86dbf5a 11 AnalogIn emg2_in( A2 );
keeswieriks 21:931fe86dbf5a 12
s1574396 23:dec549767006 13 // Constants EMG
s1574396 23:dec549767006 14 const double m1 = 0.5000;
s1574396 23:dec549767006 15 const double m2 = -0.8090;
s1574396 23:dec549767006 16 const double n0 = 0.5000;
s1574396 23:dec549767006 17 const double n1 = -0.8090;
s1574396 23:dec549767006 18 const double n2 = 0;
s1574396 23:dec549767006 19 const double a1 = 0.9565;
s1574396 23:dec549767006 20 const double a2 = -1.9131;
s1574396 23:dec549767006 21 const double b0 = 0.9565;
s1574396 23:dec549767006 22 const double b1 = -1.9112;
s1574396 23:dec549767006 23 const double b2 = 0.9150;
s1574396 23:dec549767006 24 const double c1 = 0.0675;
s1574396 23:dec549767006 25 const double c2 = 0.1349;
s1574396 23:dec549767006 26 const double d0 = 0.0675;
s1574396 23:dec549767006 27 const double d1 = -1.1430;
s1574396 23:dec549767006 28 const double d2 = 0.4128;
s1574396 24:6bdc50e21805 29
s1574396 23:dec549767006 30 // Variables EMG
s1574396 24:6bdc50e21805 31 double emg0;
s1574396 24:6bdc50e21805 32 double emg1;
s1574396 24:6bdc50e21805 33 double emg2;
s1574396 24:6bdc50e21805 34 double notch0;
s1574396 24:6bdc50e21805 35 double notch1;
s1574396 24:6bdc50e21805 36 double notch2;
s1574396 24:6bdc50e21805 37 double high0;
s1574396 24:6bdc50e21805 38 double high1;
s1574396 24:6bdc50e21805 39 double high2;
s1574396 24:6bdc50e21805 40 double absolute0;
s1574396 24:6bdc50e21805 41 double absolute1;
s1574396 24:6bdc50e21805 42 double absolute2;
s1574396 24:6bdc50e21805 43 double low0;
s1574396 24:6bdc50e21805 44 double low1;
s1574396 24:6bdc50e21805 45 double low2;
keeswieriks 21:931fe86dbf5a 46
keeswieriks 21:931fe86dbf5a 47 // BiQuad values
keeswieriks 21:931fe86dbf5a 48 BiQuadChain notch;
keeswieriks 21:931fe86dbf5a 49 BiQuad N1( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 50 BiQuad N2( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 51 BiQuad N3( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 52 BiQuadChain highpass;
keeswieriks 21:931fe86dbf5a 53 BiQuad H1( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 54 BiQuad H2( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 55 BiQuad H3( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 56 BiQuadChain lowpass;
keeswieriks 21:931fe86dbf5a 57 BiQuad L1( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 58 BiQuad L2( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 59 BiQuad L3( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 60
keeswieriks 26:1eafb6111ae8 61 // EMG
keeswieriks 26:1eafb6111ae8 62 const int sizeMovAg = 150; //Size of array over which the moving average (MovAg) is calculated
keeswieriks 26:1eafb6111ae8 63 double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
keeswieriks 26:1eafb6111ae8 64 double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};
keeswieriks 26:1eafb6111ae8 65
keeswieriks 26:1eafb6111ae8 66 //Empty arrays to calculate MovAgs
keeswieriks 26:1eafb6111ae8 67 double Average0, Average1, Average2; //Outcome of MovAg
keeswieriks 26:1eafb6111ae8 68 const int sizeCali = 2000; //Size of array over which the Threshold will be calculated
keeswieriks 26:1eafb6111ae8 69 double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};
keeswieriks 26:1eafb6111ae8 70 //Empty arrays to calculate means in calibration
keeswieriks 26:1eafb6111ae8 71
keeswieriks 26:1eafb6111ae8 72 double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
keeswieriks 26:1eafb6111ae8 73 double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
keeswieriks 26:1eafb6111ae8 74 int g = 0; //Part of the switch void, where the current state can be changed
keeswieriks 26:1eafb6111ae8 75 int emg_calib=0; //After calibration this value will be 1, enabling the
keeswieriks 26:1eafb6111ae8 76
keeswieriks 26:1eafb6111ae8 77 //EMG
keeswieriks 26:1eafb6111ae8 78 Ticker Filter_tick;
keeswieriks 26:1eafb6111ae8 79 Ticker MovAg_tick;
keeswieriks 26:1eafb6111ae8 80
s1574396 22:bcfee9594007 81 // Filter of the first EMG signal
s1574396 24:6bdc50e21805 82 void filtering()
vsluiter 2:e314bb3b2d99 83 {
s1574396 23:dec549767006 84 emg0 = emg0_in.read(); // Reading the EMG signal
s1574396 24:6bdc50e21805 85 emg1 = emg1_in.read();
s1574396 24:6bdc50e21805 86 emg2 = emg2_in.read();
s1574396 24:6bdc50e21805 87 notch0 = N1.step(emg0); // Applying a notch filter over the EMG data
s1574396 24:6bdc50e21805 88 notch1 = N2.step(emg1);
s1574396 24:6bdc50e21805 89 notch2 = N3.step(emg2);
s1574396 24:6bdc50e21805 90 high0 = H1.step(notch0); // Applying a high pass filter
s1574396 24:6bdc50e21805 91 high1 = H2.step(notch1);
s1574396 24:6bdc50e21805 92 high2 = H3.step(notch2);
s1574396 24:6bdc50e21805 93 absolute0 = fabs(high0); // Rectifying the signal
s1574396 24:6bdc50e21805 94 absolute1 = fabs(high1);
s1574396 24:6bdc50e21805 95 absolute2 = fabs(high2);
s1574396 24:6bdc50e21805 96 low0 = L1.step(absolute0); // Applying low pass filter
s1574396 24:6bdc50e21805 97 low1 = L2.step(absolute1);
s1574396 24:6bdc50e21805 98 low2 = L3.step(absolute2);
s1574396 23:dec549767006 99
keeswieriks 26:1eafb6111ae8 100 for (int i = sizeMovAg-1; i>=0; i--) {
keeswieriks 26:1eafb6111ae8 101 //For statement to make an array of the last datapoints of the filtered signal
keeswieriks 26:1eafb6111ae8 102 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right
keeswieriks 26:1eafb6111ae8 103 StoreArray1[i] = StoreArray1[i-1];
keeswieriks 26:1eafb6111ae8 104 StoreArray2[i] = StoreArray2[i-1];
keeswieriks 26:1eafb6111ae8 105 }
keeswieriks 26:1eafb6111ae8 106 StoreArray0[0] = low0; //Stores the latest datapoint in the first element of the array
keeswieriks 26:1eafb6111ae8 107 StoreArray1[0] = low1;
keeswieriks 26:1eafb6111ae8 108 StoreArray2[0] = low2;
keeswieriks 26:1eafb6111ae8 109 sum1 = 0.0;
keeswieriks 26:1eafb6111ae8 110 sum2 = 0.0;
keeswieriks 26:1eafb6111ae8 111 sum3 = 0.0;
keeswieriks 26:1eafb6111ae8 112 for (int a = 0; a<=sizeMovAg-1; a++) { //For statement to sum the elements in the array
keeswieriks 26:1eafb6111ae8 113 sum1+=StoreArray0[a];
keeswieriks 26:1eafb6111ae8 114 sum2+=StoreArray1[a];
keeswieriks 26:1eafb6111ae8 115 sum3+=StoreArray2[a];
keeswieriks 26:1eafb6111ae8 116 }
keeswieriks 26:1eafb6111ae8 117 Average0 = sum1/sizeMovAg; //Calculates an average over the datapoints in the array
keeswieriks 26:1eafb6111ae8 118 Average1 = sum2/sizeMovAg;
keeswieriks 26:1eafb6111ae8 119 Average2 = sum3/sizeMovAg;
keeswieriks 26:1eafb6111ae8 120
s1574396 25:02f183b944ed 121 scope.set( 0, emg0); // Sending the signal to the HIDScope
s1574396 25:02f183b944ed 122 scope.set( 1, low0); // Change the numer of inputs on the top when necessary
s1574396 25:02f183b944ed 123 scope.set( 2, emg1);
s1574396 25:02f183b944ed 124 scope.set( 3, low1);
s1574396 25:02f183b944ed 125 scope.set( 4, emg2);
s1574396 25:02f183b944ed 126 scope.set( 5, low2);
s1574396 24:6bdc50e21805 127 scope.send();
keeswieriks 21:931fe86dbf5a 128 }
keeswieriks 21:931fe86dbf5a 129
keeswieriks 21:931fe86dbf5a 130 int main()
keeswieriks 21:931fe86dbf5a 131 {
s1574396 25:02f183b944ed 132 sample_timer.attach( &filtering, 0.002);
keeswieriks 21:931fe86dbf5a 133
s1574396 23:dec549767006 134 while(1) {}
keeswieriks 26:1eafb6111ae8 135 }
keeswieriks 26:1eafb6111ae8 136
keeswieriks 26:1eafb6111ae8 137
keeswieriks 26:1eafb6111ae8 138
keeswieriks 26:1eafb6111ae8 139 void MovAg() //Void to make a moving average
keeswieriks 26:1eafb6111ae8 140 {
keeswieriks 26:1eafb6111ae8 141
vsluiter 0:32bb76391d89 142 }