Group 9 BioRobotics / Mbed 2 deprecated EMG2

emg2

Dependencies:   HIDScope biquadFilter mbed QEI

Fork of EMG by Tom Tom

main.cpp@30:14a9358758d1, 2018-10-31 (annotated)

Committer:
keeswieriks
Date:
Wed Oct 31 14:01:11 2018 +0000
Revision:
30:14a9358758d1
Parent:
29:9010b7bdbfbb
Child:
31:c22110c9dca4
met uitlijning er ook 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"
keeswieriks 29:9010b7bdbfbb 4 #include "QEI.h"
vsluiter 0:32bb76391d89 5
s1574396 25:02f183b944ed 6 HIDScope scope( 6 );
s1574396 22:bcfee9594007 7 Ticker sample_timer;
s1574396 22:bcfee9594007 8
keeswieriks 29:9010b7bdbfbb 9
keeswieriks 29:9010b7bdbfbb 10 Serial pc(USBTX,USBRX);
keeswieriks 29:9010b7bdbfbb 11
s1574396 23:dec549767006 12 // Inputs EMG
keeswieriks 21:931fe86dbf5a 13 AnalogIn emg0_in( A0 );
keeswieriks 21:931fe86dbf5a 14 AnalogIn emg1_in( A1 );
keeswieriks 21:931fe86dbf5a 15 AnalogIn emg2_in( A2 );
keeswieriks 21:931fe86dbf5a 16
keeswieriks 29:9010b7bdbfbb 17 InterruptIn but1(SW2);
keeswieriks 29:9010b7bdbfbb 18 InterruptIn but2(SW3);
keeswieriks 29:9010b7bdbfbb 19
keeswieriks 28:433d12c52913 20 DigitalOut led1(LED_BLUE);
keeswieriks 28:433d12c52913 21 DigitalOut led2(LED_RED);
keeswieriks 28:433d12c52913 22 DigitalOut led3(LED_GREEN);
keeswieriks 28:433d12c52913 23
s1574396 23:dec549767006 24 // Constants EMG
s1574396 23:dec549767006 25 const double m1 = 0.5000;
s1574396 23:dec549767006 26 const double m2 = -0.8090;
s1574396 23:dec549767006 27 const double n0 = 0.5000;
s1574396 23:dec549767006 28 const double n1 = -0.8090;
s1574396 23:dec549767006 29 const double n2 = 0;
s1574396 23:dec549767006 30 const double a1 = 0.9565;
s1574396 23:dec549767006 31 const double a2 = -1.9131;
s1574396 23:dec549767006 32 const double b0 = 0.9565;
s1574396 23:dec549767006 33 const double b1 = -1.9112;
s1574396 23:dec549767006 34 const double b2 = 0.9150;
s1574396 23:dec549767006 35 const double c1 = 0.0675;
s1574396 23:dec549767006 36 const double c2 = 0.1349;
s1574396 23:dec549767006 37 const double d0 = 0.0675;
s1574396 23:dec549767006 38 const double d1 = -1.1430;
s1574396 23:dec549767006 39 const double d2 = 0.4128;
s1574396 24:6bdc50e21805 40
s1574396 23:dec549767006 41 // Variables EMG
s1574396 24:6bdc50e21805 42 double emg0;
s1574396 24:6bdc50e21805 43 double emg1;
s1574396 24:6bdc50e21805 44 double emg2;
s1574396 24:6bdc50e21805 45 double notch0;
s1574396 24:6bdc50e21805 46 double notch1;
s1574396 24:6bdc50e21805 47 double notch2;
s1574396 24:6bdc50e21805 48 double high0;
s1574396 24:6bdc50e21805 49 double high1;
s1574396 24:6bdc50e21805 50 double high2;
s1574396 24:6bdc50e21805 51 double absolute0;
s1574396 24:6bdc50e21805 52 double absolute1;
s1574396 24:6bdc50e21805 53 double absolute2;
s1574396 24:6bdc50e21805 54 double low0;
s1574396 24:6bdc50e21805 55 double low1;
s1574396 24:6bdc50e21805 56 double low2;
keeswieriks 21:931fe86dbf5a 57
keeswieriks 21:931fe86dbf5a 58 // BiQuad values
keeswieriks 21:931fe86dbf5a 59 BiQuadChain notch;
keeswieriks 21:931fe86dbf5a 60 BiQuad N1( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 61 BiQuad N2( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 62 BiQuad N3( m1, m2, n0, n1, n2);
keeswieriks 21:931fe86dbf5a 63 BiQuadChain highpass;
keeswieriks 21:931fe86dbf5a 64 BiQuad H1( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 65 BiQuad H2( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 66 BiQuad H3( a1, a2, b0, b1, b2);
keeswieriks 21:931fe86dbf5a 67 BiQuadChain lowpass;
keeswieriks 21:931fe86dbf5a 68 BiQuad L1( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 69 BiQuad L2( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 70 BiQuad L3( c1, c2, d0, d1, d2);
keeswieriks 21:931fe86dbf5a 71
keeswieriks 29:9010b7bdbfbb 72 const float T=0.001f;
keeswieriks 29:9010b7bdbfbb 73
keeswieriks 26:1eafb6111ae8 74 // EMG
keeswieriks 27:6b4814ef266d 75 const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
keeswieriks 26:1eafb6111ae8 76 double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
keeswieriks 26:1eafb6111ae8 77 double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};
keeswieriks 26:1eafb6111ae8 78
keeswieriks 26:1eafb6111ae8 79 //Empty arrays to calculate MovAgs
keeswieriks 26:1eafb6111ae8 80 double Average0, Average1, Average2; //Outcome of MovAg
keeswieriks 26:1eafb6111ae8 81 const int sizeCali = 2000; //Size of array over which the Threshold will be calculated
keeswieriks 26:1eafb6111ae8 82 double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};
keeswieriks 26:1eafb6111ae8 83 //Empty arrays to calculate means in calibration
keeswieriks 26:1eafb6111ae8 84
keeswieriks 26:1eafb6111ae8 85 double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
keeswieriks 26:1eafb6111ae8 86 double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
keeswieriks 26:1eafb6111ae8 87 int g = 0; //Part of the switch void, where the current state can be changed
keeswieriks 26:1eafb6111ae8 88 int emg_calib=0; //After calibration this value will be 1, enabling the
keeswieriks 26:1eafb6111ae8 89
keeswieriks 26:1eafb6111ae8 90 //EMG
keeswieriks 26:1eafb6111ae8 91 Ticker Filter_tick;
keeswieriks 26:1eafb6111ae8 92 Ticker MovAg_tick;
keeswieriks 26:1eafb6111ae8 93
s1574396 22:bcfee9594007 94 // Filter of the first EMG signal
s1574396 24:6bdc50e21805 95 void filtering()
vsluiter 2:e314bb3b2d99 96 {
s1574396 23:dec549767006 97 emg0 = emg0_in.read(); // Reading the EMG signal
s1574396 24:6bdc50e21805 98 emg1 = emg1_in.read();
s1574396 24:6bdc50e21805 99 emg2 = emg2_in.read();
s1574396 24:6bdc50e21805 100 notch0 = N1.step(emg0); // Applying a notch filter over the EMG data
s1574396 24:6bdc50e21805 101 notch1 = N2.step(emg1);
s1574396 24:6bdc50e21805 102 notch2 = N3.step(emg2);
s1574396 24:6bdc50e21805 103 high0 = H1.step(notch0); // Applying a high pass filter
s1574396 24:6bdc50e21805 104 high1 = H2.step(notch1);
s1574396 24:6bdc50e21805 105 high2 = H3.step(notch2);
s1574396 24:6bdc50e21805 106 absolute0 = fabs(high0); // Rectifying the signal
s1574396 24:6bdc50e21805 107 absolute1 = fabs(high1);
s1574396 24:6bdc50e21805 108 absolute2 = fabs(high2);
s1574396 24:6bdc50e21805 109 low0 = L1.step(absolute0); // Applying low pass filter
s1574396 24:6bdc50e21805 110 low1 = L2.step(absolute1);
s1574396 24:6bdc50e21805 111 low2 = L3.step(absolute2);
keeswieriks 29:9010b7bdbfbb 112 }
keeswieriks 29:9010b7bdbfbb 113
keeswieriks 29:9010b7bdbfbb 114 /*void EMG_filterd()
keeswieriks 29:9010b7bdbfbb 115 {
keeswieriks 29:9010b7bdbfbb 116 low0();
keeswieriks 29:9010b7bdbfbb 117 low1();
keeswieriks 29:9010b7bdbfbb 118 low2();
keeswieriks 29:9010b7bdbfbb 119 }*/
keeswieriks 29:9010b7bdbfbb 120
keeswieriks 29:9010b7bdbfbb 121 void MovAg()
keeswieriks 29:9010b7bdbfbb 122 {
keeswieriks 26:1eafb6111ae8 123 for (int i = sizeMovAg-1; i>=0; i--) {
keeswieriks 26:1eafb6111ae8 124 //For statement to make an array of the last datapoints of the filtered signal
keeswieriks 26:1eafb6111ae8 125 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right
keeswieriks 26:1eafb6111ae8 126 StoreArray1[i] = StoreArray1[i-1];
keeswieriks 26:1eafb6111ae8 127 StoreArray2[i] = StoreArray2[i-1];
keeswieriks 26:1eafb6111ae8 128 }
keeswieriks 26:1eafb6111ae8 129 StoreArray0[0] = low0; //Stores the latest datapoint in the first element of the array
keeswieriks 26:1eafb6111ae8 130 StoreArray1[0] = low1;
keeswieriks 26:1eafb6111ae8 131 StoreArray2[0] = low2;
keeswieriks 26:1eafb6111ae8 132 sum1 = 0.0;
keeswieriks 26:1eafb6111ae8 133 sum2 = 0.0;
keeswieriks 26:1eafb6111ae8 134 sum3 = 0.0;
keeswieriks 26:1eafb6111ae8 135 for (int a = 0; a<=sizeMovAg-1; a++) { //For statement to sum the elements in the array
keeswieriks 26:1eafb6111ae8 136 sum1+=StoreArray0[a];
keeswieriks 26:1eafb6111ae8 137 sum2+=StoreArray1[a];
keeswieriks 26:1eafb6111ae8 138 sum3+=StoreArray2[a];
keeswieriks 26:1eafb6111ae8 139 }
keeswieriks 26:1eafb6111ae8 140 Average0 = sum1/sizeMovAg; //Calculates an average over the datapoints in the array
keeswieriks 26:1eafb6111ae8 141 Average1 = sum2/sizeMovAg;
keeswieriks 26:1eafb6111ae8 142 Average2 = sum3/sizeMovAg;
keeswieriks 26:1eafb6111ae8 143
s1574396 25:02f183b944ed 144 scope.set( 0, emg0); // Sending the signal to the HIDScope
s1574396 25:02f183b944ed 145 scope.set( 1, low0); // Change the numer of inputs on the top when necessary
keeswieriks 27:6b4814ef266d 146 scope.set( 2, Average0);
s1574396 25:02f183b944ed 147 scope.set( 3, low1);
s1574396 25:02f183b944ed 148 scope.set( 4, emg2);
s1574396 25:02f183b944ed 149 scope.set( 5, low2);
s1574396 24:6bdc50e21805 150 scope.send();
keeswieriks 21:931fe86dbf5a 151 }
keeswieriks 21:931fe86dbf5a 152
keeswieriks 29:9010b7bdbfbb 153 void switch_to_calibrate() //Void to switch between signals to calibrate
keeswieriks 28:433d12c52913 154 {
keeswieriks 28:433d12c52913 155 g++;
keeswieriks 28:433d12c52913 156 if (g == 0)
keeswieriks 28:433d12c52913 157 { //If g = 0, led is blue
keeswieriks 28:433d12c52913 158 led1=0;
keeswieriks 28:433d12c52913 159 led2=1;
keeswieriks 28:433d12c52913 160 led3=1;
keeswieriks 28:433d12c52913 161 }
keeswieriks 28:433d12c52913 162 else if(g == 1)
keeswieriks 28:433d12c52913 163 { //If g = 1, led is red
keeswieriks 28:433d12c52913 164 led1=1;
keeswieriks 28:433d12c52913 165 led2=0;
keeswieriks 28:433d12c52913 166 led3=1;
keeswieriks 28:433d12c52913 167
keeswieriks 28:433d12c52913 168 }
keeswieriks 28:433d12c52913 169 else if(g == 2)
keeswieriks 28:433d12c52913 170 { //If g = 2, led is green
keeswieriks 28:433d12c52913 171 led1=1;
keeswieriks 28:433d12c52913 172 led2=1;
keeswieriks 28:433d12c52913 173 led3=0;
keeswieriks 28:433d12c52913 174 }
keeswieriks 28:433d12c52913 175 else
keeswieriks 28:433d12c52913 176 { //If g > 3, led is white
keeswieriks 28:433d12c52913 177 led1=0;
keeswieriks 28:433d12c52913 178 led2=0;
keeswieriks 28:433d12c52913 179 led3=0;
keeswieriks 28:433d12c52913 180 }
keeswieriks 28:433d12c52913 181 }
keeswieriks 29:9010b7bdbfbb 182 void calibrate(void) //Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
keeswieriks 26:1eafb6111ae8 183 {
keeswieriks 30:14a9358758d1 184 switch (g)
keeswieriks 30:14a9358758d1 185 {
keeswieriks 30:14a9358758d1 186 case 0:
keeswieriks 30:14a9358758d1 187 { //Case zero, calibrate EMG signal of right biceps
keeswieriks 28:433d12c52913 188 sum = 0.0;
keeswieriks 30:14a9358758d1 189 for (int j = 0; j<=sizeCali-1; j++)
keeswieriks 30:14a9358758d1 190 {
keeswieriks 28:433d12c52913 191 //For statement to make an array of the latest datapoints of the filtered signal
keeswieriks 28:433d12c52913 192 StoreCali0[j] = low0; //Stores the latest datapoint in the first element of the array
keeswieriks 28:433d12c52913 193 sum+=StoreCali0[j]; //Sums the elements in the array
keeswieriks 28:433d12c52913 194 wait(0.001f);
keeswieriks 28:433d12c52913 195 }
keeswieriks 28:433d12c52913 196 Mean0 = sum/sizeCali; //Calculates the mean of the signal
keeswieriks 29:9010b7bdbfbb 197 Threshold0 = Mean0; //Determines the threshold (factor 0.5)
keeswieriks 28:433d12c52913 198 break;
keeswieriks 30:14a9358758d1 199 }
keeswieriks 30:14a9358758d1 200 case 1:
keeswieriks 30:14a9358758d1 201 { //Case one, calibrate EMG signal of left biceps
keeswieriks 28:433d12c52913 202 sum = 0.0;
keeswieriks 30:14a9358758d1 203 for(int j=0; j<=sizeCali-1; j++)
keeswieriks 30:14a9358758d1 204 {
keeswieriks 28:433d12c52913 205 StoreCali1[j] = low1;
keeswieriks 28:433d12c52913 206 sum+=StoreCali1[j];
keeswieriks 28:433d12c52913 207 wait(0.001f);
keeswieriks 28:433d12c52913 208 }
keeswieriks 28:433d12c52913 209 Mean1 = sum/sizeCali;
keeswieriks 29:9010b7bdbfbb 210 Threshold1 = Mean1;
keeswieriks 28:433d12c52913 211 break;
keeswieriks 30:14a9358758d1 212 }
keeswieriks 30:14a9358758d1 213 case 2:
keeswieriks 30:14a9358758d1 214 { //case two, calibrate EMG signal of calf
keeswieriks 28:433d12c52913 215 sum = 0.0;
keeswieriks 30:14a9358758d1 216 for(int j=0; j<=sizeCali-1; j++)
keeswieriks 30:14a9358758d1 217 {
keeswieriks 28:433d12c52913 218 StoreCali2[j] = low2;
keeswieriks 28:433d12c52913 219 sum+=StoreCali2[j];
keeswieriks 28:433d12c52913 220 wait(0.001f);
keeswieriks 28:433d12c52913 221 }
keeswieriks 28:433d12c52913 222 Mean2 = sum/sizeCali;
keeswieriks 29:9010b7bdbfbb 223 Threshold2 = Mean2;
keeswieriks 28:433d12c52913 224 break;
keeswieriks 30:14a9358758d1 225 }
keeswieriks 30:14a9358758d1 226 case 3:
keeswieriks 30:14a9358758d1 227 { //Sets calibration value to 1; robot can be set to Home position
keeswieriks 28:433d12c52913 228 emg_calib=1;
keeswieriks 28:433d12c52913 229 wait(0.001f);
keeswieriks 28:433d12c52913 230 break;
keeswieriks 30:14a9358758d1 231 }
keeswieriks 30:14a9358758d1 232 default:
keeswieriks 30:14a9358758d1 233 { //Ensures nothing happens if g is not equal to 0,1 or 2.
keeswieriks 30:14a9358758d1 234 break;
keeswieriks 30:14a9358758d1 235 }
keeswieriks 30:14a9358758d1 236 }
keeswieriks 29:9010b7bdbfbb 237 }
keeswieriks 29:9010b7bdbfbb 238
keeswieriks 29:9010b7bdbfbb 239
keeswieriks 29:9010b7bdbfbb 240 int main()
keeswieriks 29:9010b7bdbfbb 241 {
keeswieriks 29:9010b7bdbfbb 242 sample_timer.attach( &filtering, 0.002);
keeswieriks 29:9010b7bdbfbb 243 pc.baud(115200); // setting baudrate
keeswieriks 29:9010b7bdbfbb 244
keeswieriks 29:9010b7bdbfbb 245 while(true)
keeswieriks 29:9010b7bdbfbb 246 {
keeswieriks 29:9010b7bdbfbb 247 led1 = 0;
keeswieriks 29:9010b7bdbfbb 248 led2 = 1;
keeswieriks 29:9010b7bdbfbb 249 led3 = 1;
keeswieriks 29:9010b7bdbfbb 250 Filter_tick.attach(&filtering,T); //EMG signals filtered every T sec.
keeswieriks 29:9010b7bdbfbb 251 MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
keeswieriks 29:9010b7bdbfbb 252 //ticker.attach(&dv_des_calculate_qref,T); //v_des determined every T
keeswieriks 29:9010b7bdbfbb 253
keeswieriks 29:9010b7bdbfbb 254 // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
keeswieriks 29:9010b7bdbfbb 255
keeswieriks 29:9010b7bdbfbb 256 but1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
keeswieriks 29:9010b7bdbfbb 257 wait(0.2f); //Wait to avoid bouncing of button
keeswieriks 29:9010b7bdbfbb 258 but2.rise(calibrate); //Calibrate threshold for 3 muscles
keeswieriks 29:9010b7bdbfbb 259 wait(0.2f); //Wait to avoid bouncing of button
keeswieriks 29:9010b7bdbfbb 260
keeswieriks 29:9010b7bdbfbb 261 pc.printf("g is %i\n\r",g);
keeswieriks 29:9010b7bdbfbb 262 pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
keeswieriks 29:9010b7bdbfbb 263 pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
keeswieriks 29:9010b7bdbfbb 264 //wait(2.0f);
keeswieriks 29:9010b7bdbfbb 265 }
keeswieriks 29:9010b7bdbfbb 266 }
keeswieriks 29:9010b7bdbfbb 267
keeswieriks 29:9010b7bdbfbb 268