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

main.cpp@5:dd261ba7b047, 2016-11-03 (annotated)

Committer:
Jankoekenpan
Date:
Thu Nov 03 10:01:14 2016 +0000
Revision:
5:dd261ba7b047
Parent:
calibrate.cpp@4:4de31fc4f912 
working test version

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