Group 9 BioRobotics / Mbed 2 deprecated EMG2

emg2

Dependencies:   HIDScope biquadFilter mbed QEI

Fork of EMG by Tom Tom

main.cpp@32:4b73c693aefb, 2018-10-31 (annotated)

Committer:
s1725696
Date:
Wed Oct 31 20:28:35 2018 +0000
Revision:
32:4b73c693aefb
Parent:
31:c22110c9dca4 
final version, do not change anymore

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