Project Paint / Mbed 2 deprecated EMG-Processing

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   biquadFilter mbed

calibrate.cpp@3:082ba262d2ec, 2016-11-03 (annotated)

Committer:
ofosakar
Date:
Thu Nov 03 08:46:55 2016 +0000
Revision:
3:082ba262d2ec
Parent:
2:8b790c03a760
Child:
4:4de31fc4f912
Added function to link the EMG part with the Control part.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ofosakar 0:44d3f99b08c1 1 #include "mbed.h"
ofosakar 0:44d3f99b08c1 2 #include "BiQuad.h"
ofosakar 2:8b790c03a760 3 #include "HIDScope.h"
ofosakar 0:44d3f99b08c1 4
ofosakar 2:8b790c03a760 5 HIDScope scope(6);
ofosakar 2:8b790c03a760 6 // BUTTON USED IN CALIBRATION
ofosakar 1:984b6b6812c7 7 DigitalIn calibrating(SW2);
ofosakar 2:8b790c03a760 8 // BUTTON TO START CALIBRATING
ofosakar 1:984b6b6812c7 9 InterruptIn calibrateButton(SW3);
ofosakar 2:8b790c03a760 10 // THE TWO EMG SIGNALS
ofosakar 2:8b790c03a760 11 AnalogIn emg1(A0);
ofosakar 2:8b790c03a760 12 AnalogIn emg2(A1);
ofosakar 0:44d3f99b08c1 13 Serial pc(USBTX, USBRX);
ofosakar 0:44d3f99b08c1 14
ofosakar 2:8b790c03a760 15 // LEDS
ofosakar 0:44d3f99b08c1 16 DigitalOut led_red(LED_RED);
ofosakar 0:44d3f99b08c1 17 DigitalOut led_green(LED_GREEN);
ofosakar 1:984b6b6812c7 18 DigitalOut led_blue(LED_BLUE);
ofosakar 1:984b6b6812c7 19
ofosakar 2:8b790c03a760 20 //FOR DEBUG PURPOSES: WHEN TRUE, VALUES WILL BE WRITTEN TO HIDSCOPE
ofosakar 2:8b790c03a760 21 const bool printToHidscope = true;
ofosakar 2:8b790c03a760 22
ofosakar 2:8b790c03a760 23 // EMG BIQUAD 1
ofosakar 2:8b790c03a760 24 BiQuadChain bqc1;
ofosakar 2:8b790c03a760 25 BiQuadChain calibrateBqc1;
ofosakar 2:8b790c03a760 26 //Bandpass butterworth filter + Notch butterworth filter.
ofosakar 2:8b790c03a760 27 //No Bandpass filters
ofosakar 2:8b790c03a760 28 //Nothc: 50 +- 2 Hz
ofosakar 2:8b790c03a760 29 BiQuad calibrateBq11( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
ofosakar 2:8b790c03a760 30 BiQuad bq11( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
ofosakar 0:44d3f99b08c1 31
ofosakar 0:44d3f99b08c1 32
ofosakar 2:8b790c03a760 33 // EMG BIQUAD 2
ofosakar 2:8b790c03a760 34 BiQuadChain bqc2;
ofosakar 0:44d3f99b08c1 35 BiQuadChain calibrateBqc2;
ofosakar 2:8b790c03a760 36 //Bandpass butterworth filter + Notch butterworth filter.
ofosakar 2:8b790c03a760 37 //Bandpass: 10 --- 500 Hz
ofosakar 2:8b790c03a760 38 //No Bandpass filters
ofosakar 2:8b790c03a760 39 //Nothc: 50 +- 2 Hz
ofosakar 0:44d3f99b08c1 40 BiQuad calibrateBq12( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
ofosakar 2:8b790c03a760 41 BiQuad bq12( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
ofosakar 0:44d3f99b08c1 42
ofosakar 2:8b790c03a760 43 // ARRAYS USED IN CALIBRATING THE EMG SIGNALS
ofosakar 0:44d3f99b08c1 44 const int calibrateNumEmgCache = 100;
ofosakar 0:44d3f99b08c1 45 float calibrateEmgCache1[calibrateNumEmgCache]; //sorted from new to old;
ofosakar 0:44d3f99b08c1 46 float calibrateEmgCache2[calibrateNumEmgCache]; //sorted from new to old;
ofosakar 0:44d3f99b08c1 47
ofosakar 2:8b790c03a760 48 // ARRAYS USED IN CALCULATION OF THE MOVAG
ofosakar 2:8b790c03a760 49 const int numEmgCache = 50;
ofosakar 2:8b790c03a760 50 float emgCache1[numEmgCache]; //sorted from new to old;
ofosakar 2:8b790c03a760 51 float emgCache2[numEmgCache]; //sorted from new to old;
ofosakar 2:8b790c03a760 52
ofosakar 2:8b790c03a760 53
ofosakar 2:8b790c03a760 54 // THRESHOLDS FOR THE DECISION: BY DEFAULT 0.2,
ofosakar 2:8b790c03a760 55 // BUT SHOULD BE CHANGED IN THE CALIBRATION PHASE AT THE BEGINNING
ofosakar 1:984b6b6812c7 56 volatile float threshold1 = 0.2;
ofosakar 1:984b6b6812c7 57 volatile float threshold2 = 0.2;
ofosakar 0:44d3f99b08c1 58
ofosakar 2:8b790c03a760 59 // NUMBERS
ofosakar 2:8b790c03a760 60 int decided1[numEmgCache];
ofosakar 2:8b790c03a760 61 int decided2[numEmgCache];
ofosakar 2:8b790c03a760 62
ofosakar 2:8b790c03a760 63
ofosakar 2:8b790c03a760 64 Ticker ticker;
ofosakar 0:44d3f99b08c1 65 Ticker sampler;
ofosakar 0:44d3f99b08c1 66
ofosakar 0:44d3f99b08c1 67 float sample_frequency = 500.0f; //Hz
ofosakar 0:44d3f99b08c1 68 float Ts = 1.0f / sample_frequency;
ofosakar 2:8b790c03a760 69 // USED FOR COUNTING HOW MANY SIGNALS HAVE PASSED
ofosakar 2:8b790c03a760 70 volatile int count = 0;
ofosakar 0:44d3f99b08c1 71
ofosakar 3:082ba262d2ec 72 // FUNC TO SEND THE DATA TO THE MOTOR
ofosakar 3:082ba262d2ec 73 void (*motorFunc)(bool, bool);
ofosakar 3:082ba262d2ec 74
ofosakar 3:082ba262d2ec 75
ofosakar 2:8b790c03a760 76 ////////////////////////////////////
ofosakar 2:8b790c03a760 77 ///////// HELPER FUNCTIONS /////////
ofosakar 2:8b790c03a760 78 ////////////////////////////////////
ofosakar 2:8b790c03a760 79 void resetLeds() {
ofosakar 2:8b790c03a760 80 led_red = true;
ofosakar 2:8b790c03a760 81 led_green = true;
ofosakar 2:8b790c03a760 82 led_blue = true;
ofosakar 2:8b790c03a760 83 }
ofosakar 0:44d3f99b08c1 84
ofosakar 2:8b790c03a760 85 void addFirst(float newValue, float array[], int size) {
ofosakar 2:8b790c03a760 86 for (int i = size - 2; i >= 0; i--) {
ofosakar 2:8b790c03a760 87 array[i+1] = array[i];
ofosakar 2:8b790c03a760 88 }
ofosakar 2:8b790c03a760 89 array[0] = newValue;
ofosakar 0:44d3f99b08c1 90 }
ofosakar 2:8b790c03a760 91 void addFirst(int newValue, int array[], int size) {
ofosakar 0:44d3f99b08c1 92 for (int i = size - 2; i >= 0; i--) {
ofosakar 0:44d3f99b08c1 93 array[i+1] = array[i];
ofosakar 0:44d3f99b08c1 94 }
ofosakar 0:44d3f99b08c1 95 array[0] = newValue;
ofosakar 2:8b790c03a760 96 }
ofosakar 2:8b790c03a760 97
ofosakar 2:8b790c03a760 98 float average(float newValue, float array[], int size) {
ofosakar 2:8b790c03a760 99 float sum = 0;
ofosakar 2:8b790c03a760 100 for (int i = size - 2; i >= 0; i--) {
ofosakar 2:8b790c03a760 101 sum += array[i];
ofosakar 2:8b790c03a760 102 }
ofosakar 2:8b790c03a760 103 // array[0] = newValue;
ofosakar 2:8b790c03a760 104 sum += newValue;
ofosakar 2:8b790c03a760 105 return sum / size;
ofosakar 2:8b790c03a760 106 }
ofosakar 2:8b790c03a760 107
ofosakar 2:8b790c03a760 108 //shifts the array by adding the new emg value up front.
ofosakar 2:8b790c03a760 109 //returns the new calculated average
ofosakar 2:8b790c03a760 110 float movingAverage(float newValue, float array[], int size) {
ofosakar 2:8b790c03a760 111 float sum = 0;
ofosakar 2:8b790c03a760 112 for (int i = size - 2; i >= 0; i--) {
ofosakar 2:8b790c03a760 113 array[i+1] = array[i];
ofosakar 2:8b790c03a760 114 sum += array[i];
ofosakar 2:8b790c03a760 115 }
ofosakar 2:8b790c03a760 116 array[0] = newValue;
ofosakar 2:8b790c03a760 117 sum += newValue;
ofosakar 2:8b790c03a760 118 return sum / size;
ofosakar 0:44d3f99b08c1 119 }
ofosakar 0:44d3f99b08c1 120
ofosakar 2:8b790c03a760 121 float sum(float array[], int size) {
ofosakar 2:8b790c03a760 122 float sum = 0;
ofosakar 0:44d3f99b08c1 123 for (int i = 0; i < size; i++) {
ofosakar 2:8b790c03a760 124 sum += array[i];
ofosakar 0:44d3f99b08c1 125 }
ofosakar 2:8b790c03a760 126 return sum;
ofosakar 2:8b790c03a760 127 }
ofosakar 2:8b790c03a760 128
ofosakar 2:8b790c03a760 129 float mean(float array[], int size) {
ofosakar 2:8b790c03a760 130 return sum(array, size) / size;
ofosakar 0:44d3f99b08c1 131 }
ofosakar 0:44d3f99b08c1 132
ofosakar 2:8b790c03a760 133 float meanSquare(float array[], int size) {
ofosakar 2:8b790c03a760 134 float naam[size];
ofosakar 2:8b790c03a760 135 for(int i = 0; i < size; i++) {
ofosakar 2:8b790c03a760 136 naam[i] = pow(array[i], 2);
ofosakar 2:8b790c03a760 137 }
ofosakar 2:8b790c03a760 138 return sum(naam, size) / size;
ofosakar 0:44d3f99b08c1 139 }
ofosakar 0:44d3f99b08c1 140
ofosakar 2:8b790c03a760 141 int decide(float value, float threshold) {
ofosakar 2:8b790c03a760 142 return value < threshold ? 0 : 1;
ofosakar 2:8b790c03a760 143 }
ofosakar 2:8b790c03a760 144
ofosakar 2:8b790c03a760 145 float rectifier(float value) {
ofosakar 2:8b790c03a760 146 return fabs(value - 0.5f)*2.0f;
ofosakar 2:8b790c03a760 147 }
ofosakar 3:082ba262d2ec 148
ofosakar 3:082ba262d2ec 149
ofosakar 3:082ba262d2ec 150 void sendToMotor(void (*func)(bool, bool), bool arg1, bool arg2) {
ofosakar 3:082ba262d2ec 151 func(arg1, arg2);
ofosakar 3:082ba262d2ec 152 }
ofosakar 2:8b790c03a760 153 ////////////////////////////////////
ofosakar 2:8b790c03a760 154 ///////// HELPER FUNCTIONS /////////
ofosakar 2:8b790c03a760 155 ////////////////////////////////////
ofosakar 2:8b790c03a760 156
ofosakar 0:44d3f99b08c1 157 void sample() {
ofosakar 2:8b790c03a760 158 float emgOne = emg1.read();
ofosakar 0:44d3f99b08c1 159 float notch1 = calibrateBqc1.step( emgOne );
ofosakar 0:44d3f99b08c1 160
ofosakar 2:8b790c03a760 161 float emgTwo = emg2.read();
ofosakar 0:44d3f99b08c1 162 float notch2 = calibrateBqc2.step( emgTwo );
ofosakar 0:44d3f99b08c1 163
ofosakar 2:8b790c03a760 164 float rect1 = rectifier(notch1);
ofosakar 2:8b790c03a760 165 float rect2 = rectifier(notch2);
ofosakar 0:44d3f99b08c1 166
ofosakar 0:44d3f99b08c1 167
ofosakar 2:8b790c03a760 168 float filtered1 = movingAverage( rect1, calibrateEmgCache1, calibrateNumEmgCache);
ofosakar 2:8b790c03a760 169 float filtered2 = movingAverage( rect2, calibrateEmgCache2, calibrateNumEmgCache);
ofosakar 0:44d3f99b08c1 170 }
ofosakar 0:44d3f99b08c1 171
ofosakar 1:984b6b6812c7 172 void calibrate() {
ofosakar 1:984b6b6812c7 173 while(calibrating) {
ofosakar 1:984b6b6812c7 174 led_red = false;
ofosakar 1:984b6b6812c7 175 wait(0.5);
ofosakar 1:984b6b6812c7 176 led_red = true;
ofosakar 1:984b6b6812c7 177 wait(0.5);
ofosakar 1:984b6b6812c7 178 }
ofosakar 1:984b6b6812c7 179
ofosakar 1:984b6b6812c7 180 // Button pressed for rest measurement
ofosakar 1:984b6b6812c7 181 led_red = true;
ofosakar 1:984b6b6812c7 182 sampler.attach(&sample, Ts);
ofosakar 1:984b6b6812c7 183 led_blue = false;
ofosakar 1:984b6b6812c7 184 wait(10);
ofosakar 1:984b6b6812c7 185 // 10 seconds sampled
ofosakar 1:984b6b6812c7 186 led_blue = true;
ofosakar 1:984b6b6812c7 187 sampler.detach();
ofosakar 2:8b790c03a760 188 float restAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache);
ofosakar 2:8b790c03a760 189 float restAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache);
ofosakar 1:984b6b6812c7 190
ofosakar 1:984b6b6812c7 191 int i =0;
ofosakar 1:984b6b6812c7 192 while(i<3) {
ofosakar 1:984b6b6812c7 193 led_green = false;
ofosakar 1:984b6b6812c7 194 wait(0.5);
ofosakar 1:984b6b6812c7 195 led_green = true;
ofosakar 1:984b6b6812c7 196 wait(0.5);
ofosakar 1:984b6b6812c7 197 i++;
ofosakar 1:984b6b6812c7 198 }
ofosakar 1:984b6b6812c7 199 led_green = true;
ofosakar 1:984b6b6812c7 200
ofosakar 1:984b6b6812c7 201 while(calibrating) {
ofosakar 1:984b6b6812c7 202 led_red = false;
ofosakar 1:984b6b6812c7 203 wait(0.5);
ofosakar 1:984b6b6812c7 204 led_red = true;
ofosakar 1:984b6b6812c7 205 wait(0.5);
ofosakar 1:984b6b6812c7 206 }
ofosakar 1:984b6b6812c7 207 // Button pressed for contracted measurement
ofosakar 1:984b6b6812c7 208 led_red = true;
ofosakar 1:984b6b6812c7 209 sampler.attach(&sample, Ts);
ofosakar 1:984b6b6812c7 210 led_blue = false;
ofosakar 1:984b6b6812c7 211 wait(10);
ofosakar 1:984b6b6812c7 212
ofosakar 1:984b6b6812c7 213 // 10 seconds sampled
ofosakar 1:984b6b6812c7 214 led_blue = true;
ofosakar 1:984b6b6812c7 215 sampler.detach();
ofosakar 1:984b6b6812c7 216
ofosakar 1:984b6b6812c7 217 i =0;
ofosakar 1:984b6b6812c7 218 while(i<3) {
ofosakar 1:984b6b6812c7 219 led_green = false;
ofosakar 1:984b6b6812c7 220 wait(0.5);
ofosakar 1:984b6b6812c7 221 led_green = true;
ofosakar 1:984b6b6812c7 222 wait(0.5);
ofosakar 1:984b6b6812c7 223 i++;
ofosakar 1:984b6b6812c7 224 }
ofosakar 1:984b6b6812c7 225
ofosakar 2:8b790c03a760 226 float contAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache);
ofosakar 2:8b790c03a760 227 float contAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache);
ofosakar 1:984b6b6812c7 228
ofosakar 1:984b6b6812c7 229 threshold1 = (contAvg1 + restAvg1)/2;
ofosakar 1:984b6b6812c7 230 threshold2 = (contAvg2 + restAvg2)/2;
ofosakar 1:984b6b6812c7 231 pc.printf("threshold1: %f\tthreshold2:%f\n\r", threshold1, threshold2);
ofosakar 1:984b6b6812c7 232
ofosakar 1:984b6b6812c7 233 }
ofosakar 2:8b790c03a760 234
ofosakar 2:8b790c03a760 235 void processEMG() {
ofosakar 2:8b790c03a760 236 float emgOne = emg1.read();
ofosakar 2:8b790c03a760 237 float emgTwo = emg2.read();
ofosakar 2:8b790c03a760 238 float notch1 = bqc1.step( emgOne );
ofosakar 2:8b790c03a760 239 float notch2 = bqc2.step( emgTwo );
ofosakar 2:8b790c03a760 240
ofosakar 2:8b790c03a760 241 float rect1 = rectifier(notch1);
ofosakar 2:8b790c03a760 242 float rect2 = rectifier(notch2);
ofosakar 2:8b790c03a760 243
ofosakar 2:8b790c03a760 244 float filtered1 = movingAverage( rect1, emgCache1, numEmgCache);
ofosakar 2:8b790c03a760 245 float filtered2 = movingAverage( rect2, emgCache2, numEmgCache);
ofosakar 2:8b790c03a760 246
ofosakar 2:8b790c03a760 247 int decide1 = decide(average(filtered1, emgCache1, numEmgCache ), threshold1);
ofosakar 2:8b790c03a760 248 int decide2 = decide(average(filtered2, emgCache2, numEmgCache ), threshold2);
ofosakar 2:8b790c03a760 249 addFirst(decide1, decided1, numEmgCache);
ofosakar 2:8b790c03a760 250 addFirst(decide2, decided2, numEmgCache);
ofosakar 2:8b790c03a760 251
ofosakar 2:8b790c03a760 252 if(printToHidscope) {
ofosakar 2:8b790c03a760 253 scope.set(0,emgOne);
ofosakar 2:8b790c03a760 254 scope.set(1,emgTwo);
ofosakar 2:8b790c03a760 255 scope.set(2,decide1);
ofosakar 2:8b790c03a760 256 scope.set(3,decide2);
ofosakar 2:8b790c03a760 257 }
ofosakar 2:8b790c03a760 258
ofosakar 2:8b790c03a760 259 if (count >= 50) {
ofosakar 2:8b790c03a760 260 int counter1=0;
ofosakar 2:8b790c03a760 261 int counter2=0;
ofosakar 2:8b790c03a760 262 for(int i = 0; i < numEmgCache; ++i){
ofosakar 2:8b790c03a760 263 if(decided1[i] == 0)
ofosakar 2:8b790c03a760 264 ++counter1;
ofosakar 2:8b790c03a760 265 if(decided2[i] == 0)
ofosakar 2:8b790c03a760 266 ++counter2;
ofosakar 2:8b790c03a760 267 }
ofosakar 2:8b790c03a760 268 int avgDecide1 = counter1 > std::ceil(numEmgCache/2.0) ? 0: 1;
ofosakar 2:8b790c03a760 269 int avgDecide2 = counter2 > std::ceil(numEmgCache/2.0) ? 0: 1;
ofosakar 3:082ba262d2ec 270 sendToMotor(motorFunc,avgDecide1, avgDecide2);
ofosakar 2:8b790c03a760 271 if(printToHidscope) {
ofosakar 2:8b790c03a760 272 scope.set(4,avgDecide1);
ofosakar 2:8b790c03a760 273 scope.set(5,avgDecide2);
ofosakar 2:8b790c03a760 274 }
ofosakar 2:8b790c03a760 275 count =0;
ofosakar 2:8b790c03a760 276 } else {
ofosakar 2:8b790c03a760 277 count++;
ofosakar 2:8b790c03a760 278 }
ofosakar 2:8b790c03a760 279 scope.send();
ofosakar 2:8b790c03a760 280 }
ofosakar 2:8b790c03a760 281
ofosakar 3:082ba262d2ec 282 void consumeBools(bool x, bool y) {
ofosakar 3:082ba262d2ec 283 pc.printf("%d\t%d\n\r", x, y);
ofosakar 3:082ba262d2ec 284 }
ofosakar 1:984b6b6812c7 285 int main()
ofosakar 1:984b6b6812c7 286 {
ofosakar 1:984b6b6812c7 287 pc.baud(115200);
ofosakar 2:8b790c03a760 288
ofosakar 2:8b790c03a760 289 // initial state
ofosakar 2:8b790c03a760 290 resetLeds();
ofosakar 1:984b6b6812c7 291 calibrateButton.fall(&calibrate);
ofosakar 3:082ba262d2ec 292 // TODO CHANGE THIS TO THE DESIERD FUNCTION (THAT JAN MADE)
ofosakar 3:082ba262d2ec 293 motorFunc = &consumeBools;
ofosakar 2:8b790c03a760 294
ofosakar 2:8b790c03a760 295 // how to call the calibrating function
ofosakar 1:984b6b6812c7 296 calibrate();
ofosakar 1:984b6b6812c7 297
ofosakar 2:8b790c03a760 298
ofosakar 2:8b790c03a760 299 bqc1.add( &bq11 );
ofosakar 2:8b790c03a760 300 bqc2.add( &bq12 );
ofosakar 2:8b790c03a760 301
ofosakar 2:8b790c03a760 302 // 500 HZ Ticker
ofosakar 2:8b790c03a760 303 ticker.attach(&processEMG, 0.002);
ofosakar 2:8b790c03a760 304 while (true);
ofosakar 2:8b790c03a760 305 }