Group 9 BioRobotics / Mbed 2 deprecated EMG2

emg2

Dependencies:   HIDScope biquadFilter mbed QEI

Fork of EMG by Tom Tom

main.cpp@28:433d12c52913, 2018-10-30 (annotated)

Committer:
keeswieriks
Date:
Tue Oct 30 10:23:46 2018 +0000
Revision:
28:433d12c52913
Parent:
27:6b4814ef266d
Child:
29:9010b7bdbfbb
met emg cali er bijna bij; ;

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
keeswieriks 28:433d12c52913 13 DigitalOut led1(LED_BLUE);
keeswieriks 28:433d12c52913 14 DigitalOut led2(LED_RED);
keeswieriks 28:433d12c52913 15 DigitalOut led3(LED_GREEN);
keeswieriks 28:433d12c52913 16
s1574396 23:dec549767006 17 // Constants EMG
s1574396 23:dec549767006 18 const double m1 = 0.5000;
s1574396 23:dec549767006 19 const double m2 = -0.8090;
s1574396 23:dec549767006 20 const double n0 = 0.5000;
s1574396 23:dec549767006 21 const double n1 = -0.8090;
s1574396 23:dec549767006 22 const double n2 = 0;
s1574396 23:dec549767006 23 const double a1 = 0.9565;
s1574396 23:dec549767006 24 const double a2 = -1.9131;
s1574396 23:dec549767006 25 const double b0 = 0.9565;
s1574396 23:dec549767006 26 const double b1 = -1.9112;
s1574396 23:dec549767006 27 const double b2 = 0.9150;
s1574396 23:dec549767006 28 const double c1 = 0.0675;
s1574396 23:dec549767006 29 const double c2 = 0.1349;
s1574396 23:dec549767006 30 const double d0 = 0.0675;
s1574396 23:dec549767006 31 const double d1 = -1.1430;
s1574396 23:dec549767006 32 const double d2 = 0.4128;
s1574396 24:6bdc50e21805 33
s1574396 23:dec549767006 34 // Variables EMG
s1574396 24:6bdc50e21805 35 double emg0;
s1574396 24:6bdc50e21805 36 double emg1;
s1574396 24:6bdc50e21805 37 double emg2;
s1574396 24:6bdc50e21805 38 double notch0;
s1574396 24:6bdc50e21805 39 double notch1;
s1574396 24:6bdc50e21805 40 double notch2;
s1574396 24:6bdc50e21805 41 double high0;
s1574396 24:6bdc50e21805 42 double high1;
s1574396 24:6bdc50e21805 43 double high2;
s1574396 24:6bdc50e21805 44 double absolute0;
s1574396 24:6bdc50e21805 45 double absolute1;
s1574396 24:6bdc50e21805 46 double absolute2;
s1574396 24:6bdc50e21805 47 double low0;
s1574396 24:6bdc50e21805 48 double low1;
s1574396 24:6bdc50e21805 49 double low2;
keeswieriks 21:931fe86dbf5a 50
keeswieriks 21:931fe86dbf5a 51 // BiQuad values
keeswieriks 21:931fe86dbf5a 52 BiQuadChain notch;
keeswieriks 21:931fe86dbf5a 53 BiQuad N1( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 54 BiQuad N2( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 55 BiQuad N3( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 56 BiQuadChain highpass;
keeswieriks 21:931fe86dbf5a 57 BiQuad H1( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 58 BiQuad H2( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 59 BiQuad H3( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 60 BiQuadChain lowpass;
keeswieriks 21:931fe86dbf5a 61 BiQuad L1( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 62 BiQuad L2( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 63 BiQuad L3( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 64
keeswieriks 26:1eafb6111ae8 65 // EMG
keeswieriks 27:6b4814ef266d 66 const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
keeswieriks 26:1eafb6111ae8 67 double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
keeswieriks 26:1eafb6111ae8 68 double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};
keeswieriks 26:1eafb6111ae8 69
keeswieriks 26:1eafb6111ae8 70 //Empty arrays to calculate MovAgs
keeswieriks 26:1eafb6111ae8 71 double Average0, Average1, Average2; //Outcome of MovAg
keeswieriks 26:1eafb6111ae8 72 const int sizeCali = 2000; //Size of array over which the Threshold will be calculated
keeswieriks 26:1eafb6111ae8 73 double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};
keeswieriks 26:1eafb6111ae8 74 //Empty arrays to calculate means in calibration
keeswieriks 26:1eafb6111ae8 75
keeswieriks 26:1eafb6111ae8 76 double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
keeswieriks 26:1eafb6111ae8 77 double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
keeswieriks 26:1eafb6111ae8 78 int g = 0; //Part of the switch void, where the current state can be changed
keeswieriks 26:1eafb6111ae8 79 int emg_calib=0; //After calibration this value will be 1, enabling the
keeswieriks 26:1eafb6111ae8 80
keeswieriks 26:1eafb6111ae8 81 //EMG
keeswieriks 26:1eafb6111ae8 82 Ticker Filter_tick;
keeswieriks 26:1eafb6111ae8 83 Ticker MovAg_tick;
keeswieriks 26:1eafb6111ae8 84
s1574396 22:bcfee9594007 85 // Filter of the first EMG signal
s1574396 24:6bdc50e21805 86 void filtering()
vsluiter 2:e314bb3b2d99 87 {
s1574396 23:dec549767006 88 emg0 = emg0_in.read(); // Reading the EMG signal
s1574396 24:6bdc50e21805 89 emg1 = emg1_in.read();
s1574396 24:6bdc50e21805 90 emg2 = emg2_in.read();
s1574396 24:6bdc50e21805 91 notch0 = N1.step(emg0); // Applying a notch filter over the EMG data
s1574396 24:6bdc50e21805 92 notch1 = N2.step(emg1);
s1574396 24:6bdc50e21805 93 notch2 = N3.step(emg2);
s1574396 24:6bdc50e21805 94 high0 = H1.step(notch0); // Applying a high pass filter
s1574396 24:6bdc50e21805 95 high1 = H2.step(notch1);
s1574396 24:6bdc50e21805 96 high2 = H3.step(notch2);
s1574396 24:6bdc50e21805 97 absolute0 = fabs(high0); // Rectifying the signal
s1574396 24:6bdc50e21805 98 absolute1 = fabs(high1);
s1574396 24:6bdc50e21805 99 absolute2 = fabs(high2);
s1574396 24:6bdc50e21805 100 low0 = L1.step(absolute0); // Applying low pass filter
s1574396 24:6bdc50e21805 101 low1 = L2.step(absolute1);
s1574396 24:6bdc50e21805 102 low2 = L3.step(absolute2);
s1574396 23:dec549767006 103
keeswieriks 26:1eafb6111ae8 104 for (int i = sizeMovAg-1; i>=0; i--) {
keeswieriks 26:1eafb6111ae8 105 //For statement to make an array of the last datapoints of the filtered signal
keeswieriks 26:1eafb6111ae8 106 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right
keeswieriks 26:1eafb6111ae8 107 StoreArray1[i] = StoreArray1[i-1];
keeswieriks 26:1eafb6111ae8 108 StoreArray2[i] = StoreArray2[i-1];
keeswieriks 26:1eafb6111ae8 109 }
keeswieriks 26:1eafb6111ae8 110 StoreArray0[0] = low0; //Stores the latest datapoint in the first element of the array
keeswieriks 26:1eafb6111ae8 111 StoreArray1[0] = low1;
keeswieriks 26:1eafb6111ae8 112 StoreArray2[0] = low2;
keeswieriks 26:1eafb6111ae8 113 sum1 = 0.0;
keeswieriks 26:1eafb6111ae8 114 sum2 = 0.0;
keeswieriks 26:1eafb6111ae8 115 sum3 = 0.0;
keeswieriks 26:1eafb6111ae8 116 for (int a = 0; a<=sizeMovAg-1; a++) { //For statement to sum the elements in the array
keeswieriks 26:1eafb6111ae8 117 sum1+=StoreArray0[a];
keeswieriks 26:1eafb6111ae8 118 sum2+=StoreArray1[a];
keeswieriks 26:1eafb6111ae8 119 sum3+=StoreArray2[a];
keeswieriks 26:1eafb6111ae8 120 }
keeswieriks 26:1eafb6111ae8 121 Average0 = sum1/sizeMovAg; //Calculates an average over the datapoints in the array
keeswieriks 26:1eafb6111ae8 122 Average1 = sum2/sizeMovAg;
keeswieriks 26:1eafb6111ae8 123 Average2 = sum3/sizeMovAg;
keeswieriks 26:1eafb6111ae8 124
s1574396 25:02f183b944ed 125 scope.set( 0, emg0); // Sending the signal to the HIDScope
s1574396 25:02f183b944ed 126 scope.set( 1, low0); // Change the numer of inputs on the top when necessary
keeswieriks 27:6b4814ef266d 127 scope.set( 2, Average0);
s1574396 25:02f183b944ed 128 scope.set( 3, low1);
s1574396 25:02f183b944ed 129 scope.set( 4, emg2);
s1574396 25:02f183b944ed 130 scope.set( 5, low2);
s1574396 24:6bdc50e21805 131 scope.send();
keeswieriks 21:931fe86dbf5a 132 }
keeswieriks 21:931fe86dbf5a 133
keeswieriks 21:931fe86dbf5a 134 int main()
keeswieriks 21:931fe86dbf5a 135 {
s1574396 25:02f183b944ed 136 sample_timer.attach( &filtering, 0.002);
keeswieriks 21:931fe86dbf5a 137
s1574396 23:dec549767006 138 while(1) {}
keeswieriks 26:1eafb6111ae8 139 }
keeswieriks 26:1eafb6111ae8 140
keeswieriks 26:1eafb6111ae8 141
keeswieriks 26:1eafb6111ae8 142
keeswieriks 28:433d12c52913 143 void Switching() //Void to switch between signals to calibrate
keeswieriks 28:433d12c52913 144 {
keeswieriks 28:433d12c52913 145 g++;
keeswieriks 28:433d12c52913 146 if (g == 0)
keeswieriks 28:433d12c52913 147 { //If g = 0, led is blue
keeswieriks 28:433d12c52913 148 led1=0;
keeswieriks 28:433d12c52913 149 led2=1;
keeswieriks 28:433d12c52913 150 led3=1;
keeswieriks 28:433d12c52913 151 }
keeswieriks 28:433d12c52913 152 else if(g == 1)
keeswieriks 28:433d12c52913 153 { //If g = 1, led is red
keeswieriks 28:433d12c52913 154 led1=1;
keeswieriks 28:433d12c52913 155 led2=0;
keeswieriks 28:433d12c52913 156 led3=1;
keeswieriks 28:433d12c52913 157
keeswieriks 28:433d12c52913 158 }
keeswieriks 28:433d12c52913 159 else if(g == 2)
keeswieriks 28:433d12c52913 160 { //If g = 2, led is green
keeswieriks 28:433d12c52913 161 led1=1;
keeswieriks 28:433d12c52913 162 led2=1;
keeswieriks 28:433d12c52913 163 led3=0;
keeswieriks 28:433d12c52913 164 }
keeswieriks 28:433d12c52913 165 else
keeswieriks 28:433d12c52913 166 { //If g > 3, led is white
keeswieriks 28:433d12c52913 167 led1=0;
keeswieriks 28:433d12c52913 168 led2=0;
keeswieriks 28:433d12c52913 169 led3=0;
keeswieriks 28:433d12c52913 170 }
keeswieriks 28:433d12c52913 171 }
keeswieriks 28:433d12c52913 172 void Calibration(void) //Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
keeswieriks 28:433d12c52913 173
keeswieriks 26:1eafb6111ae8 174 {
keeswieriks 28:433d12c52913 175 switch (g) {
keeswieriks 28:433d12c52913 176 case 0: { //Case zero, calibrate EMG signal of right biceps
keeswieriks 28:433d12c52913 177 sum = 0.0;
keeswieriks 28:433d12c52913 178 for (int j = 0; j<=sizeCali-1; j++) {
keeswieriks 28:433d12c52913 179 //For statement to make an array of the latest datapoints of the filtered signal
keeswieriks 28:433d12c52913 180 StoreCali0[j] = low0; //Stores the latest datapoint in the first element of the array
keeswieriks 28:433d12c52913 181 sum+=StoreCali0[j]; //Sums the elements in the array
keeswieriks 28:433d12c52913 182 wait(0.001f);
keeswieriks 28:433d12c52913 183 }
keeswieriks 28:433d12c52913 184 Mean0 = sum/sizeCali; //Calculates the mean of the signal
keeswieriks 28:433d12c52913 185 Threshold0 = Mean0/2; //Determines the threshold (factor 0.5)
keeswieriks 28:433d12c52913 186 break;
keeswieriks 28:433d12c52913 187 }
keeswieriks 28:433d12c52913 188 case 1: { //Case one, calibrate EMG signal of left biceps
keeswieriks 28:433d12c52913 189 sum = 0.0;
keeswieriks 28:433d12c52913 190 for(int j=0; j<=sizeCali-1; j++) {
keeswieriks 28:433d12c52913 191 StoreCali1[j] = low1;
keeswieriks 28:433d12c52913 192 sum+=StoreCali1[j];
keeswieriks 28:433d12c52913 193 wait(0.001f);
keeswieriks 28:433d12c52913 194 }
keeswieriks 28:433d12c52913 195 Mean1 = sum/sizeCali;
keeswieriks 28:433d12c52913 196 Threshold1 = Mean1/2;
keeswieriks 28:433d12c52913 197 break;
keeswieriks 28:433d12c52913 198 }
keeswieriks 28:433d12c52913 199 case 2: { //case two, calibrate EMG signal of calf
keeswieriks 28:433d12c52913 200 sum = 0.0;
keeswieriks 28:433d12c52913 201 for(int j=0; j<=sizeCali-1; j++) {
keeswieriks 28:433d12c52913 202 StoreCali2[j] = low2;
keeswieriks 28:433d12c52913 203 sum+=StoreCali2[j];
keeswieriks 28:433d12c52913 204 wait(0.001f);
keeswieriks 28:433d12c52913 205 }
keeswieriks 28:433d12c52913 206 Mean2 = sum/sizeCali;
keeswieriks 28:433d12c52913 207 Threshold2 = Mean2/2;
keeswieriks 28:433d12c52913 208 break;
keeswieriks 28:433d12c52913 209 }
keeswieriks 28:433d12c52913 210 case 3: { //Sets calibration value to 1; robot can be set to Home position
keeswieriks 28:433d12c52913 211 emg_calib=1;
keeswieriks 28:433d12c52913 212 wait(0.001f);
keeswieriks 28:433d12c52913 213 break;
keeswieriks 28:433d12c52913 214 }
keeswieriks 28:433d12c52913 215 default: { //Ensures nothing happens if g is not equal to 0,1 or 2.
keeswieriks 28:433d12c52913 216 break;
keeswieriks 28:433d12c52913 217 }
keeswieriks 28:433d12c52913 218 }
vsluiter 0:32bb76391d89 219 }