EMG filtering; highpass, notch, abs, moving average

Dependencies:   HIDScope MODSERIAL- mbed-dsp mbed

Committer:
Hooglugt
Date:
Wed Oct 22 09:23:09 2014 +0000
Revision:
42:f45e4dfbc26d
Parent:
41:245f33fb2b8b
EMG hoogdoorlaat, notch, abs en 10e orde moving average

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hooglugt 23:8d9a623dd713 1 #include "mbed.h"
Hooglugt 23:8d9a623dd713 2 #include "MODSERIAL.h"
Hooglugt 23:8d9a623dd713 3 #include "HIDScope.h"
Hooglugt 40:7e93c2f1c1e9 4 #include "arm_math.h"
Hooglugt 29:7523e4a8e000 5
Hooglugt 39:0328b1f16a5a 6 #define TIMEB4NEXTCHOICE 1 // sec keuzelampje blijft aan
Hooglugt 39:0328b1f16a5a 7 #define TIMEBETWEENBLINK 20 // sec voor volgende blink
Hooglugt 23:8d9a623dd713 8
Hooglugt 23:8d9a623dd713 9 //Define objects
Hooglugt 29:7523e4a8e000 10 AnalogIn emg0(PTB1); //Analog input biceps
Hooglugt 40:7e93c2f1c1e9 11 float filemg_bifloat; //filtered emg-waarde biceps
Hooglugt 38:277ba1c0693c 12
Hooglugt 29:7523e4a8e000 13 AnalogIn emg1(PTB2); //Analog input triceps
Hooglugt 40:7e93c2f1c1e9 14 float filemg_trifloat; //filtered emg-waarde triceps
Hooglugt 29:7523e4a8e000 15
Hooglugt 38:277ba1c0693c 16 Ticker log_timer;
Hooglugt 38:277ba1c0693c 17 Ticker reset_timer;
Hooglugt 38:277ba1c0693c 18 MODSERIAL pc(USBTX,USBRX);
Hooglugt 39:0328b1f16a5a 19 HIDScope scope(2);
Hooglugt 29:7523e4a8e000 20
Hooglugt 41:245f33fb2b8b 21 arm_biquad_casd_df1_inst_f32 highpass;
Hooglugt 41:245f33fb2b8b 22 float highpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
Hooglugt 41:245f33fb2b8b 23 float highpass_states[4];
Hooglugt 40:7e93c2f1c1e9 24
Hooglugt 41:245f33fb2b8b 25 arm_biquad_casd_df1_inst_f32 notch;
Hooglugt 41:245f33fb2b8b 26 float notch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
Hooglugt 41:245f33fb2b8b 27 float notch_states[4];
Hooglugt 40:7e93c2f1c1e9 28
Hooglugt 41:245f33fb2b8b 29 // variables for biceps MAF
Hooglugt 41:245f33fb2b8b 30 float y0 = 0;
Hooglugt 41:245f33fb2b8b 31 float y1 = 0;
Hooglugt 41:245f33fb2b8b 32 float y2 = 0;
Hooglugt 41:245f33fb2b8b 33 float y3 = 0;
Hooglugt 41:245f33fb2b8b 34 float y4 = 0;
Hooglugt 41:245f33fb2b8b 35 float y5 = 0;
Hooglugt 41:245f33fb2b8b 36 float y6 = 0;
Hooglugt 41:245f33fb2b8b 37 float y7 = 0;
Hooglugt 41:245f33fb2b8b 38 float y8 = 0;
Hooglugt 41:245f33fb2b8b 39 float y9 = 0;
Hooglugt 40:7e93c2f1c1e9 40
Hooglugt 41:245f33fb2b8b 41 // variables for triceps MAF
Hooglugt 41:245f33fb2b8b 42 float x0 = 0;
Hooglugt 41:245f33fb2b8b 43 float x1 = 0;
Hooglugt 41:245f33fb2b8b 44 float x2 = 0;
Hooglugt 41:245f33fb2b8b 45 float x3 = 0;
Hooglugt 41:245f33fb2b8b 46 float x4 = 0;
Hooglugt 41:245f33fb2b8b 47 float x5 = 0;
Hooglugt 41:245f33fb2b8b 48 float x6 = 0;
Hooglugt 41:245f33fb2b8b 49 float x7 = 0;
Hooglugt 41:245f33fb2b8b 50 float x8 = 0;
Hooglugt 41:245f33fb2b8b 51 float x9 = 0;
Hooglugt 40:7e93c2f1c1e9 52
Hooglugt 29:7523e4a8e000 53 PwmOut red(LED_RED);
Hooglugt 23:8d9a623dd713 54 PwmOut green(LED_GREEN);
Hooglugt 23:8d9a623dd713 55 PwmOut blue(LED_BLUE);
Hooglugt 29:7523e4a8e000 56
Hooglugt 40:7e93c2f1c1e9 57 uint8_t direction = 0;
Hooglugt 40:7e93c2f1c1e9 58 uint8_t force = 0;
Hooglugt 23:8d9a623dd713 59
Hooglugt 41:245f33fb2b8b 60 void looper()
Hooglugt 23:8d9a623dd713 61 {
Hooglugt 39:0328b1f16a5a 62 //put raw emg value of biceps and triceps in emg_bifloat and emg_trifloat, respectively
Hooglugt 40:7e93c2f1c1e9 63 float emg_bifloat; //Float voor EMG-waarde biceps
Hooglugt 40:7e93c2f1c1e9 64 float emg_trifloat; //Float voor EMG-waarde triceps
Hooglugt 38:277ba1c0693c 65 emg_bifloat = emg0.read(); // read float value (0..1 = 0..3.3V) biceps
Hooglugt 38:277ba1c0693c 66 emg_trifloat = emg1.read(); // read float value (0..1 = 0..3.3V) triceps
Hooglugt 41:245f33fb2b8b 67
Hooglugt 40:7e93c2f1c1e9 68 //process emg biceps
Hooglugt 41:245f33fb2b8b 69 arm_biquad_cascade_df1_f32(&highpass, &emg_bifloat, &filemg_bifloat, 1 );
Hooglugt 41:245f33fb2b8b 70 arm_biquad_cascade_df1_f32(&notch, &emg_bifloat, &filemg_bifloat, 1 );
Hooglugt 41:245f33fb2b8b 71 y0 = fabs(filemg_bifloat);
Hooglugt 41:245f33fb2b8b 72 float bi_result = y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1;
Hooglugt 41:245f33fb2b8b 73 y9=y8;
Hooglugt 41:245f33fb2b8b 74 y8=y7;
Hooglugt 41:245f33fb2b8b 75 y7=y6;
Hooglugt 41:245f33fb2b8b 76 y6=y5;
Hooglugt 41:245f33fb2b8b 77 y5=y4;
Hooglugt 41:245f33fb2b8b 78 y4=y3;
Hooglugt 41:245f33fb2b8b 79 y3=y2;
Hooglugt 41:245f33fb2b8b 80 y2=y1;
Hooglugt 41:245f33fb2b8b 81 y1=y0;
Hooglugt 41:245f33fb2b8b 82
Hooglugt 40:7e93c2f1c1e9 83 //process emg triceps
Hooglugt 41:245f33fb2b8b 84 arm_biquad_cascade_df1_f32(&highpass, &emg_trifloat, &filemg_trifloat, 1 );
Hooglugt 41:245f33fb2b8b 85 arm_biquad_cascade_df1_f32(&notch, &emg_trifloat, &filemg_trifloat, 1 );
Hooglugt 41:245f33fb2b8b 86 x0 = fabs(filemg_trifloat);
Hooglugt 41:245f33fb2b8b 87 float tri_result = x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1;
Hooglugt 41:245f33fb2b8b 88 x9=x8;
Hooglugt 41:245f33fb2b8b 89 x8=x7;
Hooglugt 41:245f33fb2b8b 90 x7=x6;
Hooglugt 41:245f33fb2b8b 91 x6=x5;
Hooglugt 41:245f33fb2b8b 92 x5=x4;
Hooglugt 41:245f33fb2b8b 93 x4=x3;
Hooglugt 41:245f33fb2b8b 94 x3=x2;
Hooglugt 41:245f33fb2b8b 95 x2=x1;
Hooglugt 41:245f33fb2b8b 96 x1=x0;
Hooglugt 29:7523e4a8e000 97
Hooglugt 31:d8eaf0ce8517 98 /*send value to PC. Line below is used to prevent buffer overrun */
Hooglugt 41:245f33fb2b8b 99 if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) {
Hooglugt 41:245f33fb2b8b 100 scope.set(0,bi_result);
Hooglugt 41:245f33fb2b8b 101 scope.set(1,tri_result);
Hooglugt 41:245f33fb2b8b 102 scope.send();
Hooglugt 29:7523e4a8e000 103 }
Hooglugt 23:8d9a623dd713 104 }
Hooglugt 23:8d9a623dd713 105
Hooglugt 34:e166187e62ce 106 /*
Hooglugt 32:aaf01b1bf05d 107 void resetlooper() // VRAAG: wat gebeurt er wanneer en resetlooper en looper tegelijkertijd gecalled worden?!
Hooglugt 31:d8eaf0ce8517 108 {
Hooglugt 41:245f33fb2b8b 109 if(filemg_trifloat.read()>0.8 && direction != 0) { //dit is alleen mogelijk wanneer directionchoice is gemaakt
Hooglugt 30:5e5098b0cca6 110 direction = 0;
Hooglugt 30:5e5098b0cca6 111 force = 0; // WEGHALEN, wanneer in uiteindelijke script na force keuzen niet meer gereset kan worden (voor nu wel handig)
Hooglugt 31:d8eaf0ce8517 112 pc.printf("reset ");
Hooglugt 31:d8eaf0ce8517 113 }
Hooglugt 30:5e5098b0cca6 114 }
Hooglugt 41:245f33fb2b8b 115 */
Hooglugt 31:d8eaf0ce8517 116
Hooglugt 23:8d9a623dd713 117 int main()
Hooglugt 23:8d9a623dd713 118 {
Hooglugt 25:ec41b972b250 119 pc.baud(115200); //baudrate instellen
Hooglugt 40:7e93c2f1c1e9 120 log_timer.attach(looper, 0.002); // The looper() function will be called every 0.002 seconds (with the ticker object)
Hooglugt 41:245f33fb2b8b 121 //set up filters
Hooglugt 41:245f33fb2b8b 122 arm_biquad_cascade_df1_init_f32(&notch, 1, notch_const, notch_states);
Hooglugt 41:245f33fb2b8b 123 arm_biquad_cascade_df1_init_f32(&highpass, 1, highpass_const, highpass_states);
Hooglugt 41:245f33fb2b8b 124
Hooglugt 41:245f33fb2b8b 125
Hooglugt 34:e166187e62ce 126 // reset_timer.attach(resetlooper, 0.1); //
Hooglugt 32:aaf01b1bf05d 127
Hooglugt 37:2d248e64b745 128 goto directionchoice; // goes to first while(1) for the deciding the direction
Hooglugt 37:2d248e64b745 129
Hooglugt 37:2d248e64b745 130 while(1) { //Loop keuze DIRECTION
Hooglugt 37:2d248e64b745 131 directionchoice:
Hooglugt 23:8d9a623dd713 132 for(int i=1; i<4; i++) {
Hooglugt 23:8d9a623dd713 133 if(i==1) { //red
Hooglugt 23:8d9a623dd713 134 red=0;
Hooglugt 23:8d9a623dd713 135 green=1;
Hooglugt 23:8d9a623dd713 136 blue=1;
Hooglugt 37:2d248e64b745 137 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 138 if(filemg_bifloat>0.8) { // 0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 25:ec41b972b250 139 direction = 1;
Hooglugt 37:2d248e64b745 140 red=1;
Hooglugt 37:2d248e64b745 141 green = 0;
Hooglugt 36:af949aaaba01 142 blue = 0;
Hooglugt 37:2d248e64b745 143 pc.printf("links ");
Hooglugt 37:2d248e64b745 144 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 37:2d248e64b745 145 goto forcechoice; // goes to second while(1) for the deciding the force
Hooglugt 23:8d9a623dd713 146 } else {
Hooglugt 23:8d9a623dd713 147 wait(0.1);
Hooglugt 23:8d9a623dd713 148 }
Hooglugt 23:8d9a623dd713 149 }
Hooglugt 23:8d9a623dd713 150 }
Hooglugt 23:8d9a623dd713 151 if(i==2) { //green
Hooglugt 23:8d9a623dd713 152 red =1;
Hooglugt 23:8d9a623dd713 153 green=0;
Hooglugt 23:8d9a623dd713 154 blue=1;
Hooglugt 37:2d248e64b745 155 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 156 if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 25:ec41b972b250 157 direction = 2;
Hooglugt 37:2d248e64b745 158 red=0;
Hooglugt 29:7523e4a8e000 159 green = 1;
Hooglugt 37:2d248e64b745 160 blue = 0;
Hooglugt 37:2d248e64b745 161 pc.printf("mid ");
Hooglugt 37:2d248e64b745 162 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 23:8d9a623dd713 163 goto forcechoice;
Hooglugt 23:8d9a623dd713 164 } else {
Hooglugt 23:8d9a623dd713 165 wait(0.1);
Hooglugt 23:8d9a623dd713 166 }
Hooglugt 23:8d9a623dd713 167 }
Hooglugt 23:8d9a623dd713 168 }
Hooglugt 23:8d9a623dd713 169 if(i==3) { //blue
Hooglugt 23:8d9a623dd713 170 red=1;
Hooglugt 23:8d9a623dd713 171 green=1;
Hooglugt 23:8d9a623dd713 172 blue=0;
Hooglugt 37:2d248e64b745 173 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 174 if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 23:8d9a623dd713 175 direction = 3;
Hooglugt 37:2d248e64b745 176 red=0;
Hooglugt 29:7523e4a8e000 177 green = 0;
Hooglugt 37:2d248e64b745 178 blue = 1;
Hooglugt 37:2d248e64b745 179 pc.printf("rechts ");
Hooglugt 37:2d248e64b745 180 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 23:8d9a623dd713 181 goto forcechoice;
Hooglugt 23:8d9a623dd713 182 } else {
Hooglugt 23:8d9a623dd713 183 wait(0.1);
Hooglugt 23:8d9a623dd713 184 }
Hooglugt 23:8d9a623dd713 185 }
Hooglugt 23:8d9a623dd713 186 }
Hooglugt 25:ec41b972b250 187 }
Hooglugt 23:8d9a623dd713 188 }
Hooglugt 37:2d248e64b745 189 while(1) { //Loop keuze FORCE
Hooglugt 25:ec41b972b250 190 forcechoice:
Hooglugt 37:2d248e64b745 191 for(int j=1; j<4; j++) {
Hooglugt 37:2d248e64b745 192 if(j==1) { //red
Hooglugt 37:2d248e64b745 193 red=0;
Hooglugt 37:2d248e64b745 194 green=1;
Hooglugt 37:2d248e64b745 195 blue=1;
Hooglugt 37:2d248e64b745 196 if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is)
Hooglugt 37:2d248e64b745 197 goto directionchoice;
Hooglugt 37:2d248e64b745 198 }
Hooglugt 37:2d248e64b745 199 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 200 if(filemg_bifloat>0.8) { // 0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 37:2d248e64b745 201 force = 1;
Hooglugt 37:2d248e64b745 202 red=1;
Hooglugt 37:2d248e64b745 203 green = 0;
Hooglugt 37:2d248e64b745 204 blue = 0;
Hooglugt 37:2d248e64b745 205 pc.printf("zwak ");
Hooglugt 37:2d248e64b745 206 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 37:2d248e64b745 207 goto choicesmade;
Hooglugt 37:2d248e64b745 208 } else {
Hooglugt 37:2d248e64b745 209 wait(0.1);
Hooglugt 23:8d9a623dd713 210 }
Hooglugt 23:8d9a623dd713 211 }
Hooglugt 37:2d248e64b745 212 }
Hooglugt 37:2d248e64b745 213 if(j==2) { //green
Hooglugt 37:2d248e64b745 214 red =1;
Hooglugt 37:2d248e64b745 215 green=0;
Hooglugt 37:2d248e64b745 216 blue=1;
Hooglugt 37:2d248e64b745 217 if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is)
Hooglugt 37:2d248e64b745 218 goto directionchoice;
Hooglugt 37:2d248e64b745 219 }
Hooglugt 37:2d248e64b745 220 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 221 if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 37:2d248e64b745 222 force = 2;
Hooglugt 37:2d248e64b745 223 red=0;
Hooglugt 37:2d248e64b745 224 green = 1;
Hooglugt 37:2d248e64b745 225 blue = 0;
Hooglugt 37:2d248e64b745 226 pc.printf("normaal ");
Hooglugt 37:2d248e64b745 227 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 37:2d248e64b745 228 goto choicesmade;
Hooglugt 37:2d248e64b745 229 } else {
Hooglugt 37:2d248e64b745 230 wait(0.1);
Hooglugt 23:8d9a623dd713 231 }
Hooglugt 23:8d9a623dd713 232 }
Hooglugt 37:2d248e64b745 233 }
Hooglugt 37:2d248e64b745 234 if(j==3) { //blue
Hooglugt 37:2d248e64b745 235 red=1;
Hooglugt 37:2d248e64b745 236 green=1;
Hooglugt 37:2d248e64b745 237 blue=0;
Hooglugt 37:2d248e64b745 238 if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is)
Hooglugt 37:2d248e64b745 239 goto directionchoice;
Hooglugt 37:2d248e64b745 240 }
Hooglugt 37:2d248e64b745 241 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 242 if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 37:2d248e64b745 243 force = 3;
Hooglugt 37:2d248e64b745 244 red=0;
Hooglugt 37:2d248e64b745 245 green = 0;
Hooglugt 37:2d248e64b745 246 blue = 1;
Hooglugt 37:2d248e64b745 247 pc.printf("sterk ");
Hooglugt 37:2d248e64b745 248 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 37:2d248e64b745 249 goto choicesmade;
Hooglugt 37:2d248e64b745 250 } else {
Hooglugt 37:2d248e64b745 251 wait(0.1);
Hooglugt 23:8d9a623dd713 252 }
Hooglugt 23:8d9a623dd713 253 }
Hooglugt 23:8d9a623dd713 254 }
Hooglugt 23:8d9a623dd713 255 }
Hooglugt 37:2d248e64b745 256 }
Hooglugt 37:2d248e64b745 257 choicesmade:
Hooglugt 33:a78ec776dfd5 258
Hooglugt 40:7e93c2f1c1e9 259 red = 0;
Hooglugt 40:7e93c2f1c1e9 260 green = 0;
Hooglugt 40:7e93c2f1c1e9 261 blue = 0; // wit lampje
Hooglugt 40:7e93c2f1c1e9 262
Hooglugt 37:2d248e64b745 263 /* Vanaf hier komt de aansturing van de motor (inclusief de controller)*/
Hooglugt 37:2d248e64b745 264
Hooglugt 33:a78ec776dfd5 265
Hooglugt 25:ec41b972b250 266 if(direction == 1 && force == 1) { // links zwak
Hooglugt 39:0328b1f16a5a 267 pc.printf("links zwak ");
Hooglugt 40:7e93c2f1c1e9 268 wait(2);
Hooglugt 25:ec41b972b250 269 }
Hooglugt 25:ec41b972b250 270 if(direction == 1 && force == 2) { // links normaal
Hooglugt 39:0328b1f16a5a 271 pc.printf("links normaal ");
Hooglugt 25:ec41b972b250 272 }
Hooglugt 25:ec41b972b250 273 if(direction == 1 && force == 3) { // links sterk
Hooglugt 39:0328b1f16a5a 274 pc.printf("links sterk ");
Hooglugt 25:ec41b972b250 275 }
Hooglugt 25:ec41b972b250 276 if(direction == 2 && force == 1) { // mid zwak
Hooglugt 39:0328b1f16a5a 277 pc.printf("mid zwak ");
Hooglugt 25:ec41b972b250 278 }
Hooglugt 25:ec41b972b250 279 if(direction == 2 && force == 2) { // mid normaal
Hooglugt 39:0328b1f16a5a 280 pc.printf("mid normaal ");
Hooglugt 25:ec41b972b250 281 }
Hooglugt 25:ec41b972b250 282 if(direction == 2 && force == 3) { // mid sterk
Hooglugt 39:0328b1f16a5a 283 pc.printf("mid sterk ");
Hooglugt 25:ec41b972b250 284 }
Hooglugt 25:ec41b972b250 285 if(direction == 3 && force == 1) { // rechts zwak
Hooglugt 39:0328b1f16a5a 286 pc.printf("rechts zwak ");
Hooglugt 25:ec41b972b250 287 }
Hooglugt 25:ec41b972b250 288 if(direction == 3 && force == 2) { // rechts normaal
Hooglugt 39:0328b1f16a5a 289 pc.printf("rechts normaal ");
Hooglugt 25:ec41b972b250 290 }
Hooglugt 25:ec41b972b250 291 if(direction == 3 && force == 3) { // rechts sterk
Hooglugt 39:0328b1f16a5a 292 pc.printf("rechts sterk ");
Hooglugt 25:ec41b972b250 293 }
Hooglugt 33:a78ec776dfd5 294 if(direction == 0 || force == 0) { // wanneer de triceps in de korte tijd is aangespannen nadat beide keuzes zijn gemaakt
Hooglugt 39:0328b1f16a5a 295 pc.printf("error ");
Hooglugt 33:a78ec776dfd5 296 // mogelijkheid om een goto te maken naar directionchoice (opzich wel handig)
Hooglugt 32:aaf01b1bf05d 297 }
Hooglugt 41:245f33fb2b8b 298
Hooglugt 25:ec41b972b250 299 }