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@4:4de31fc4f912, 2016-11-03 (annotated)

Committer:
Jankoekenpan
Date:
Thu Nov 03 09:19:45 2016 +0000
Revision:
4:4de31fc4f912
Parent:
3:082ba262d2ec 
remove useless average function; add some documentation

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
Jankoekenpan 4:4de31fc4f912 49 // Values in these arrays contain samples that are already notched and rectified.
ofosakar 2:8b790c03a760 50 const int numEmgCache = 50;
ofosakar 2:8b790c03a760 51 float emgCache1[numEmgCache]; //sorted from new to old;
ofosakar 2:8b790c03a760 52 float emgCache2[numEmgCache]; //sorted from new to old;
ofosakar 2:8b790c03a760 53
ofosakar 2:8b790c03a760 54
ofosakar 2:8b790c03a760 55 // THRESHOLDS FOR THE DECISION: BY DEFAULT 0.2,
ofosakar 2:8b790c03a760 56 // BUT SHOULD BE CHANGED IN THE CALIBRATION PHASE AT THE BEGINNING
ofosakar 1:984b6b6812c7 57 volatile float threshold1 = 0.2;
ofosakar 1:984b6b6812c7 58 volatile float threshold2 = 0.2;
ofosakar 0:44d3f99b08c1 59
ofosakar 2:8b790c03a760 60 // NUMBERS
ofosakar 2:8b790c03a760 61 int decided1[numEmgCache];
ofosakar 2:8b790c03a760 62 int decided2[numEmgCache];
ofosakar 2:8b790c03a760 63
ofosakar 2:8b790c03a760 64
ofosakar 2:8b790c03a760 65 Ticker ticker;
ofosakar 0:44d3f99b08c1 66 Ticker sampler;
ofosakar 0:44d3f99b08c1 67
ofosakar 0:44d3f99b08c1 68 float sample_frequency = 500.0f; //Hz
ofosakar 0:44d3f99b08c1 69 float Ts = 1.0f / sample_frequency;
ofosakar 2:8b790c03a760 70 // USED FOR COUNTING HOW MANY SIGNALS HAVE PASSED
ofosakar 2:8b790c03a760 71 volatile int count = 0;
ofosakar 0:44d3f99b08c1 72
ofosakar 3:082ba262d2ec 73 // FUNC TO SEND THE DATA TO THE MOTOR
ofosakar 3:082ba262d2ec 74 void (*motorFunc)(bool, bool);
ofosakar 3:082ba262d2ec 75
ofosakar 3:082ba262d2ec 76
ofosakar 2:8b790c03a760 77 ////////////////////////////////////
ofosakar 2:8b790c03a760 78 ///////// HELPER FUNCTIONS /////////
ofosakar 2:8b790c03a760 79 ////////////////////////////////////
ofosakar 2:8b790c03a760 80 void resetLeds() {
ofosakar 2:8b790c03a760 81 led_red = true;
ofosakar 2:8b790c03a760 82 led_green = true;
ofosakar 2:8b790c03a760 83 led_blue = true;
ofosakar 2:8b790c03a760 84 }
ofosakar 0:44d3f99b08c1 85
ofosakar 2:8b790c03a760 86 void addFirst(float newValue, float array[], int size) {
ofosakar 2:8b790c03a760 87 for (int i = size - 2; i >= 0; i--) {
ofosakar 2:8b790c03a760 88 array[i+1] = array[i];
ofosakar 2:8b790c03a760 89 }
ofosakar 2:8b790c03a760 90 array[0] = newValue;
ofosakar 0:44d3f99b08c1 91 }
ofosakar 2:8b790c03a760 92 void addFirst(int newValue, int array[], int size) {
ofosakar 0:44d3f99b08c1 93 for (int i = size - 2; i >= 0; i--) {
ofosakar 0:44d3f99b08c1 94 array[i+1] = array[i];
ofosakar 0:44d3f99b08c1 95 }
ofosakar 0:44d3f99b08c1 96 array[0] = newValue;
ofosakar 2:8b790c03a760 97 }
ofosakar 2:8b790c03a760 98
ofosakar 2:8b790c03a760 99 //shifts the array by adding the new emg value up front.
ofosakar 2:8b790c03a760 100 //returns the new calculated average
ofosakar 2:8b790c03a760 101 float movingAverage(float newValue, float array[], int size) {
ofosakar 2:8b790c03a760 102 float sum = 0;
ofosakar 2:8b790c03a760 103 for (int i = size - 2; i >= 0; i--) {
ofosakar 2:8b790c03a760 104 array[i+1] = array[i];
ofosakar 2:8b790c03a760 105 sum += array[i];
ofosakar 2:8b790c03a760 106 }
ofosakar 2:8b790c03a760 107 array[0] = newValue;
ofosakar 2:8b790c03a760 108 sum += newValue;
ofosakar 2:8b790c03a760 109 return sum / size;
ofosakar 0:44d3f99b08c1 110 }
ofosakar 0:44d3f99b08c1 111
ofosakar 2:8b790c03a760 112 float sum(float array[], int size) {
ofosakar 2:8b790c03a760 113 float sum = 0;
ofosakar 0:44d3f99b08c1 114 for (int i = 0; i < size; i++) {
ofosakar 2:8b790c03a760 115 sum += array[i];
ofosakar 0:44d3f99b08c1 116 }
ofosakar 2:8b790c03a760 117 return sum;
ofosakar 2:8b790c03a760 118 }
ofosakar 2:8b790c03a760 119
ofosakar 2:8b790c03a760 120 float mean(float array[], int size) {
ofosakar 2:8b790c03a760 121 return sum(array, size) / size;
ofosakar 0:44d3f99b08c1 122 }
ofosakar 0:44d3f99b08c1 123
ofosakar 2:8b790c03a760 124 float meanSquare(float array[], int size) {
ofosakar 2:8b790c03a760 125 float naam[size];
ofosakar 2:8b790c03a760 126 for(int i = 0; i < size; i++) {
ofosakar 2:8b790c03a760 127 naam[i] = pow(array[i], 2);
ofosakar 2:8b790c03a760 128 }
ofosakar 2:8b790c03a760 129 return sum(naam, size) / size;
ofosakar 0:44d3f99b08c1 130 }
ofosakar 0:44d3f99b08c1 131
ofosakar 2:8b790c03a760 132 int decide(float value, float threshold) {
ofosakar 2:8b790c03a760 133 return value < threshold ? 0 : 1;
ofosakar 2:8b790c03a760 134 }
ofosakar 2:8b790c03a760 135
ofosakar 2:8b790c03a760 136 float rectifier(float value) {
ofosakar 2:8b790c03a760 137 return fabs(value - 0.5f)*2.0f;
ofosakar 2:8b790c03a760 138 }
ofosakar 3:082ba262d2ec 139
ofosakar 3:082ba262d2ec 140
ofosakar 3:082ba262d2ec 141 void sendToMotor(void (*func)(bool, bool), bool arg1, bool arg2) {
ofosakar 3:082ba262d2ec 142 func(arg1, arg2);
ofosakar 3:082ba262d2ec 143 }
ofosakar 2:8b790c03a760 144 ////////////////////////////////////
ofosakar 2:8b790c03a760 145 ///////// HELPER FUNCTIONS /////////
ofosakar 2:8b790c03a760 146 ////////////////////////////////////
ofosakar 2:8b790c03a760 147
ofosakar 0:44d3f99b08c1 148 void sample() {
ofosakar 2:8b790c03a760 149 float emgOne = emg1.read();
ofosakar 0:44d3f99b08c1 150 float notch1 = calibrateBqc1.step( emgOne );
ofosakar 0:44d3f99b08c1 151
ofosakar 2:8b790c03a760 152 float emgTwo = emg2.read();
ofosakar 0:44d3f99b08c1 153 float notch2 = calibrateBqc2.step( emgTwo );
ofosakar 0:44d3f99b08c1 154
ofosakar 2:8b790c03a760 155 float rect1 = rectifier(notch1);
ofosakar 2:8b790c03a760 156 float rect2 = rectifier(notch2);
ofosakar 0:44d3f99b08c1 157
ofosakar 0:44d3f99b08c1 158
ofosakar 2:8b790c03a760 159 float filtered1 = movingAverage( rect1, calibrateEmgCache1, calibrateNumEmgCache);
ofosakar 2:8b790c03a760 160 float filtered2 = movingAverage( rect2, calibrateEmgCache2, calibrateNumEmgCache);
ofosakar 0:44d3f99b08c1 161 }
ofosakar 0:44d3f99b08c1 162
ofosakar 1:984b6b6812c7 163 void calibrate() {
ofosakar 1:984b6b6812c7 164 while(calibrating) {
ofosakar 1:984b6b6812c7 165 led_red = false;
ofosakar 1:984b6b6812c7 166 wait(0.5);
ofosakar 1:984b6b6812c7 167 led_red = true;
ofosakar 1:984b6b6812c7 168 wait(0.5);
ofosakar 1:984b6b6812c7 169 }
ofosakar 1:984b6b6812c7 170
ofosakar 1:984b6b6812c7 171 // Button pressed for rest measurement
ofosakar 1:984b6b6812c7 172 led_red = true;
ofosakar 1:984b6b6812c7 173 sampler.attach(&sample, Ts);
ofosakar 1:984b6b6812c7 174 led_blue = false;
ofosakar 1:984b6b6812c7 175 wait(10);
ofosakar 1:984b6b6812c7 176 // 10 seconds sampled
ofosakar 1:984b6b6812c7 177 led_blue = true;
ofosakar 1:984b6b6812c7 178 sampler.detach();
ofosakar 2:8b790c03a760 179 float restAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache);
ofosakar 2:8b790c03a760 180 float restAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache);
ofosakar 1:984b6b6812c7 181
ofosakar 1:984b6b6812c7 182 int i =0;
ofosakar 1:984b6b6812c7 183 while(i<3) {
ofosakar 1:984b6b6812c7 184 led_green = false;
ofosakar 1:984b6b6812c7 185 wait(0.5);
ofosakar 1:984b6b6812c7 186 led_green = true;
ofosakar 1:984b6b6812c7 187 wait(0.5);
ofosakar 1:984b6b6812c7 188 i++;
ofosakar 1:984b6b6812c7 189 }
ofosakar 1:984b6b6812c7 190 led_green = true;
ofosakar 1:984b6b6812c7 191
ofosakar 1:984b6b6812c7 192 while(calibrating) {
ofosakar 1:984b6b6812c7 193 led_red = false;
ofosakar 1:984b6b6812c7 194 wait(0.5);
ofosakar 1:984b6b6812c7 195 led_red = true;
ofosakar 1:984b6b6812c7 196 wait(0.5);
ofosakar 1:984b6b6812c7 197 }
ofosakar 1:984b6b6812c7 198 // Button pressed for contracted measurement
ofosakar 1:984b6b6812c7 199 led_red = true;
ofosakar 1:984b6b6812c7 200 sampler.attach(&sample, Ts);
ofosakar 1:984b6b6812c7 201 led_blue = false;
ofosakar 1:984b6b6812c7 202 wait(10);
ofosakar 1:984b6b6812c7 203
ofosakar 1:984b6b6812c7 204 // 10 seconds sampled
ofosakar 1:984b6b6812c7 205 led_blue = true;
ofosakar 1:984b6b6812c7 206 sampler.detach();
ofosakar 1:984b6b6812c7 207
ofosakar 1:984b6b6812c7 208 i =0;
ofosakar 1:984b6b6812c7 209 while(i<3) {
ofosakar 1:984b6b6812c7 210 led_green = false;
ofosakar 1:984b6b6812c7 211 wait(0.5);
ofosakar 1:984b6b6812c7 212 led_green = true;
ofosakar 1:984b6b6812c7 213 wait(0.5);
ofosakar 1:984b6b6812c7 214 i++;
ofosakar 1:984b6b6812c7 215 }
ofosakar 1:984b6b6812c7 216
ofosakar 2:8b790c03a760 217 float contAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache);
ofosakar 2:8b790c03a760 218 float contAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache);
ofosakar 1:984b6b6812c7 219
ofosakar 1:984b6b6812c7 220 threshold1 = (contAvg1 + restAvg1)/2;
ofosakar 1:984b6b6812c7 221 threshold2 = (contAvg2 + restAvg2)/2;
ofosakar 1:984b6b6812c7 222 pc.printf("threshold1: %f\tthreshold2:%f\n\r", threshold1, threshold2);
ofosakar 1:984b6b6812c7 223
ofosakar 1:984b6b6812c7 224 }
ofosakar 2:8b790c03a760 225
ofosakar 2:8b790c03a760 226 void processEMG() {
ofosakar 2:8b790c03a760 227 float emgOne = emg1.read();
ofosakar 2:8b790c03a760 228 float emgTwo = emg2.read();
ofosakar 2:8b790c03a760 229 float notch1 = bqc1.step( emgOne );
ofosakar 2:8b790c03a760 230 float notch2 = bqc2.step( emgTwo );
ofosakar 2:8b790c03a760 231
ofosakar 2:8b790c03a760 232 float rect1 = rectifier(notch1);
ofosakar 2:8b790c03a760 233 float rect2 = rectifier(notch2);
ofosakar 2:8b790c03a760 234
ofosakar 2:8b790c03a760 235 float filtered1 = movingAverage( rect1, emgCache1, numEmgCache);
ofosakar 2:8b790c03a760 236 float filtered2 = movingAverage( rect2, emgCache2, numEmgCache);
ofosakar 2:8b790c03a760 237
Jankoekenpan 4:4de31fc4f912 238 int decide1 = decide(mean(emgCache1, numEmgCache ), threshold1);
Jankoekenpan 4:4de31fc4f912 239 int decide2 = decide(mean(emgCache2, numEmgCache ), threshold2);
ofosakar 2:8b790c03a760 240 addFirst(decide1, decided1, numEmgCache);
ofosakar 2:8b790c03a760 241 addFirst(decide2, decided2, numEmgCache);
ofosakar 2:8b790c03a760 242
ofosakar 2:8b790c03a760 243 if(printToHidscope) {
ofosakar 2:8b790c03a760 244 scope.set(0,emgOne);
ofosakar 2:8b790c03a760 245 scope.set(1,emgTwo);
ofosakar 2:8b790c03a760 246 scope.set(2,decide1);
ofosakar 2:8b790c03a760 247 scope.set(3,decide2);
ofosakar 2:8b790c03a760 248 }
ofosakar 2:8b790c03a760 249
Jankoekenpan 4:4de31fc4f912 250 if (count >= 49) {
ofosakar 2:8b790c03a760 251 int counter1=0;
ofosakar 2:8b790c03a760 252 int counter2=0;
ofosakar 2:8b790c03a760 253 for(int i = 0; i < numEmgCache; ++i){
ofosakar 2:8b790c03a760 254 if(decided1[i] == 0)
ofosakar 2:8b790c03a760 255 ++counter1;
ofosakar 2:8b790c03a760 256 if(decided2[i] == 0)
ofosakar 2:8b790c03a760 257 ++counter2;
ofosakar 2:8b790c03a760 258 }
ofosakar 2:8b790c03a760 259 int avgDecide1 = counter1 > std::ceil(numEmgCache/2.0) ? 0: 1;
ofosakar 2:8b790c03a760 260 int avgDecide2 = counter2 > std::ceil(numEmgCache/2.0) ? 0: 1;
ofosakar 3:082ba262d2ec 261 sendToMotor(motorFunc,avgDecide1, avgDecide2);
ofosakar 2:8b790c03a760 262 if(printToHidscope) {
ofosakar 2:8b790c03a760 263 scope.set(4,avgDecide1);
ofosakar 2:8b790c03a760 264 scope.set(5,avgDecide2);
ofosakar 2:8b790c03a760 265 }
ofosakar 2:8b790c03a760 266 count =0;
ofosakar 2:8b790c03a760 267 } else {
ofosakar 2:8b790c03a760 268 count++;
ofosakar 2:8b790c03a760 269 }
ofosakar 2:8b790c03a760 270 scope.send();
ofosakar 2:8b790c03a760 271 }
ofosakar 2:8b790c03a760 272
ofosakar 3:082ba262d2ec 273 void consumeBools(bool x, bool y) {
ofosakar 3:082ba262d2ec 274 pc.printf("%d\t%d\n\r", x, y);
ofosakar 3:082ba262d2ec 275 }
ofosakar 1:984b6b6812c7 276 int main()
ofosakar 1:984b6b6812c7 277 {
ofosakar 1:984b6b6812c7 278 pc.baud(115200);
ofosakar 2:8b790c03a760 279
ofosakar 2:8b790c03a760 280 // initial state
ofosakar 2:8b790c03a760 281 resetLeds();
ofosakar 1:984b6b6812c7 282 calibrateButton.fall(&calibrate);
ofosakar 3:082ba262d2ec 283 // TODO CHANGE THIS TO THE DESIERD FUNCTION (THAT JAN MADE)
ofosakar 3:082ba262d2ec 284 motorFunc = &consumeBools;
ofosakar 2:8b790c03a760 285
ofosakar 2:8b790c03a760 286 // how to call the calibrating function
ofosakar 1:984b6b6812c7 287 calibrate();
ofosakar 1:984b6b6812c7 288
ofosakar 2:8b790c03a760 289
ofosakar 2:8b790c03a760 290 bqc1.add( &bq11 );
ofosakar 2:8b790c03a760 291 bqc2.add( &bq12 );
ofosakar 2:8b790c03a760 292
ofosakar 2:8b790c03a760 293 // 500 HZ Ticker
ofosakar 2:8b790c03a760 294 ticker.attach(&processEMG, 0.002);
ofosakar 2:8b790c03a760 295 while (true);
ofosakar 2:8b790c03a760 296 }