Rick Poppe / Mbed 2 deprecated EMG_filtering

the emg filtering part of the program

Dependencies:   HIDScope biquadFilter mbed MODSERIAL

Fork of EMG by Tom Tom

main.cpp@23:54d28f9eef53, 2016-10-19 (annotated)

Committer:
RiP
Date:
Wed Oct 19 20:56:47 2016 +0000
Revision:
23:54d28f9eef53
Parent:
22:f38a15e851d2
Child:
24:01b4b51b5dc6
working statements

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"
RiP 21:3aecd735319d 3 #include "BiQuad.h"
RiP 22:f38a15e851d2 4 #include "MODSERIAL.h"
RiP 23:54d28f9eef53 5 #include <string.h>
vsluiter 0:32bb76391d89 6
vsluiter 4:8b298dfada81 7 //Define objects
RiP 21:3aecd735319d 8 AnalogIn emg1( A0 );
RiP 21:3aecd735319d 9 AnalogIn emg2( A1 );
RiP 21:3aecd735319d 10 AnalogIn emg3( A2 );
tomlankhorst 19:2bf824669684 11
tomlankhorst 14:f83354387756 12 Ticker sample_timer;
RiP 21:3aecd735319d 13 HIDScope scope( 6 );
RiP 22:f38a15e851d2 14 MODSERIAL pc(USBTX, USBRX);
tomlankhorst 18:21d8e7a81cf5 15 DigitalOut led(LED1);
vsluiter 2:e314bb3b2d99 16
RiP 22:f38a15e851d2 17 volatile bool sampletimer = false;
RiP 23:54d28f9eef53 18 double threshold = 0.04;
RiP 23:54d28f9eef53 19 double samplefreq=0.002;
RiP 22:f38a15e851d2 20
RiP 23:54d28f9eef53 21 typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states;
RiP 22:f38a15e851d2 22
RiP 21:3aecd735319d 23 BiQuadChain bqc11;
RiP 21:3aecd735319d 24 BiQuadChain bqc13;
RiP 21:3aecd735319d 25 BiQuadChain bqc21;
RiP 21:3aecd735319d 26 BiQuadChain bqc23;
RiP 21:3aecd735319d 27 BiQuadChain bqc31;
RiP 21:3aecd735319d 28 BiQuadChain bqc33;
RiP 21:3aecd735319d 29 //BiQuad bq11( 9.87589e-01, -1.59795e+00, 9.87589e-01, -1.59795e+00, 9.75178e-01 ); //oude BiQuad waardes
RiP 21:3aecd735319d 30 /* BiQuads for filter emg1
RiP 21:3aecd735319d 31 notch filter*/
RiP 21:3aecd735319d 32 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 33 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 34 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 21:3aecd735319d 35 /* High pass filter*/
RiP 21:3aecd735319d 36 BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
RiP 21:3aecd735319d 37 /* low pass filter*/
RiP 21:3aecd735319d 38 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 39
RiP 21:3aecd735319d 40
RiP 21:3aecd735319d 41 /* BiQuads for filter emg2
RiP 21:3aecd735319d 42 notch filter*/
RiP 21:3aecd735319d 43 BiQuad bq211(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 44 BiQuad bq212(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 45 BiQuad bq213(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 21:3aecd735319d 46 /* High pass filter*/
RiP 21:3aecd735319d 47 BiQuad bq221( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
RiP 21:3aecd735319d 48 /* low pass filter*/
RiP 21:3aecd735319d 49 BiQuad bq231( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 50
RiP 21:3aecd735319d 51 /* BiQuads for filter emg3
RiP 21:3aecd735319d 52 notch filter*/
RiP 21:3aecd735319d 53 BiQuad bq311(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 54 BiQuad bq312(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 55 BiQuad bq313(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 21:3aecd735319d 56 /* High pass filter*/
RiP 21:3aecd735319d 57 BiQuad bq321( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
RiP 21:3aecd735319d 58 /* low pass filter*/
RiP 21:3aecd735319d 59 BiQuad bq331( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 60
RiP 22:f38a15e851d2 61
RiP 22:f38a15e851d2 62 void sampleflag()
RiP 22:f38a15e851d2 63 {
RiP 22:f38a15e851d2 64 sampletimer=true;
RiP 22:f38a15e851d2 65 }
RiP 22:f38a15e851d2 66
RiP 22:f38a15e851d2 67
RiP 23:54d28f9eef53 68 void sample(states &mystate)
vsluiter 2:e314bb3b2d99 69 {
RiP 23:54d28f9eef53 70
RiP 21:3aecd735319d 71 /* Read the emg signals and filter it*/
RiP 22:f38a15e851d2 72
RiP 21:3aecd735319d 73 scope.set(0, emg1.read()); //original signal
RiP 21:3aecd735319d 74 double emg02=bqc13.step(fabs(bqc11.step(emg1.read())));
RiP 21:3aecd735319d 75 scope.set(1, emg02);
RiP 21:3aecd735319d 76 /* Read the second emg signal and filter it*/
RiP 21:3aecd735319d 77 scope.set(2, emg2.read()); //original signal
RiP 21:3aecd735319d 78 double emg12=bqc23.step(fabs(bqc21.step(emg2.read())));
RiP 21:3aecd735319d 79 scope.set(3, emg12);
RiP 21:3aecd735319d 80 /* Read the third emg signal and filter it*/
RiP 21:3aecd735319d 81 scope.set(4, emg3.read()); //original signal
RiP 21:3aecd735319d 82 double emg22=bqc33.step(fabs(bqc31.step(emg3.read())));
RiP 21:3aecd735319d 83 scope.set(5, emg22);
RiP 22:f38a15e851d2 84 //pc.printf("Hello World!\n");
RiP 21:3aecd735319d 85 /* Ensure that enough channels are available (HIDScope scope( 2 ))
RiP 21:3aecd735319d 86 * Finally, send all channels to the PC at once */
vsluiter 11:ce72ec658a95 87 scope.send();
RiP 21:3aecd735319d 88 /* To indicate that the function is working, the LED is toggled */
tomlankhorst 18:21d8e7a81cf5 89 led = !led;
RiP 23:54d28f9eef53 90 if (emg02>threshold) {
RiP 23:54d28f9eef53 91 if (emg12>threshold&&emg22>threshold) {
RiP 23:54d28f9eef53 92 mystate = STATE_XY_NEG;
RiP 23:54d28f9eef53 93 } else if (emg12>threshold) {
RiP 23:54d28f9eef53 94 mystate = STATE_X_NEG;
RiP 23:54d28f9eef53 95
RiP 23:54d28f9eef53 96 } else if (emg22>threshold) {
RiP 23:54d28f9eef53 97 mystate = STATE_Y_NEG;
RiP 23:54d28f9eef53 98 } else {
RiP 23:54d28f9eef53 99 mystate = STATE_PAUZE;
RiP 23:54d28f9eef53 100 }
RiP 22:f38a15e851d2 101 } else {
RiP 23:54d28f9eef53 102 if (emg12>threshold&&emg22>threshold) {
RiP 23:54d28f9eef53 103 mystate = STATE_XY;
RiP 23:54d28f9eef53 104 } else if (emg12>threshold) {
RiP 23:54d28f9eef53 105 mystate = STATE_X;
RiP 23:54d28f9eef53 106
RiP 23:54d28f9eef53 107 } else if (emg22>threshold) {
RiP 23:54d28f9eef53 108 mystate = STATE_Y;
RiP 23:54d28f9eef53 109 } else {
RiP 23:54d28f9eef53 110 mystate = STATE_PAUZE;
RiP 23:54d28f9eef53 111 }
RiP 22:f38a15e851d2 112 }
RiP 23:54d28f9eef53 113
RiP 22:f38a15e851d2 114 sampletimer = false;
vsluiter 2:e314bb3b2d99 115 }
vsluiter 0:32bb76391d89 116
vsluiter 0:32bb76391d89 117 int main()
RiP 21:3aecd735319d 118 {
RiP 22:f38a15e851d2 119 pc.baud(115200);
RiP 22:f38a15e851d2 120
RiP 23:54d28f9eef53 121 // create a variable called 'state', define it
RiP 23:54d28f9eef53 122 states mystate = STATE_CALIBRATION;
RiP 23:54d28f9eef53 123
RiP 21:3aecd735319d 124 //de biquad chains instellen
RiP 21:3aecd735319d 125 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 );
RiP 21:3aecd735319d 126 bqc13.add( &bq131);
RiP 21:3aecd735319d 127 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 );
RiP 21:3aecd735319d 128 bqc23.add( &bq231);
RiP 21:3aecd735319d 129 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 );
RiP 21:3aecd735319d 130 bqc33.add( &bq331);
RiP 21:3aecd735319d 131 /*Attach the 'sample' function to the timer 'sample_timer'.
RiP 21:3aecd735319d 132 this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
vsluiter 4:8b298dfada81 133 */
RiP 23:54d28f9eef53 134 sample_timer.attach(&sampleflag, samplefreq);
tomlankhorst 15:0da764eea774 135
RiP 22:f38a15e851d2 136 while(1) {
RiP 22:f38a15e851d2 137 if (sampletimer==true) {
RiP 23:54d28f9eef53 138 sample(mystate);
RiP 22:f38a15e851d2 139
RiP 23:54d28f9eef53 140 // switch, case
RiP 23:54d28f9eef53 141 switch (mystate) {
RiP 23:54d28f9eef53 142 case STATE_CALIBRATION : // calibration
RiP 23:54d28f9eef53 143 pc.printf("calibration");
RiP 23:54d28f9eef53 144 wait(0.5f);
RiP 23:54d28f9eef53 145 break;
RiP 23:54d28f9eef53 146 case STATE_X : // run
RiP 23:54d28f9eef53 147 pc.printf("X");
RiP 23:54d28f9eef53 148 break;
RiP 23:54d28f9eef53 149 case STATE_X_NEG : // run
RiP 23:54d28f9eef53 150 pc.printf("Xneg");
RiP 23:54d28f9eef53 151 break;
RiP 23:54d28f9eef53 152 case STATE_Y : // execute mode 1
RiP 23:54d28f9eef53 153 pc.printf("Y");
RiP 23:54d28f9eef53 154 break;
RiP 23:54d28f9eef53 155 case STATE_Y_NEG : // execute mode 1
RiP 23:54d28f9eef53 156 pc.printf("Yneg");
RiP 23:54d28f9eef53 157 break;
RiP 23:54d28f9eef53 158 case STATE_XY : // execute mode 2
RiP 23:54d28f9eef53 159 pc.printf("XY");
RiP 23:54d28f9eef53 160 break;
RiP 23:54d28f9eef53 161 case STATE_XY_NEG : // execute mode 2
RiP 23:54d28f9eef53 162 pc.printf("XYneg");
RiP 23:54d28f9eef53 163 break;
RiP 23:54d28f9eef53 164 case STATE_PAUZE : // default
RiP 23:54d28f9eef53 165 pc.printf("pauze");
RiP 23:54d28f9eef53 166 break;
RiP 23:54d28f9eef53 167 }
RiP 22:f38a15e851d2 168 }
RiP 22:f38a15e851d2 169 }
vsluiter 0:32bb76391d89 170 }