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@24:01b4b51b5dc6, 2016-10-20 (annotated)

Committer:
RiP
Date:
Thu Oct 20 08:26:41 2016 +0000
Revision:
24:01b4b51b5dc6
Parent:
23:54d28f9eef53
Child:
25:1a71424b05ff
STATE _CALIBRATION with interrupt

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