Aukie Hooglugt / Mbed 2 deprecated EMG_Project

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:   HIDScope MODSERIAL- mbed-dsp mbed

Project_main.cpp@40:7e93c2f1c1e9, 2014-10-10 (annotated)

Committer:
Hooglugt
Date:
Fri Oct 10 09:34:43 2014 +0000
Revision:
40:7e93c2f1c1e9
Parent:
39:0328b1f16a5a
Child:
41:245f33fb2b8b
added filtering, constants not correct yet - changed sample rate to 500 Hz

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 40:7e93c2f1c1e9 21 arm_biquad_casd_df1_inst_f32 bilowpass;
Hooglugt 40:7e93c2f1c1e9 22 float bilowpass_const[] = {0.02008337 , 0.04016673 , 0.02008337 , 1.56101808 , -0.64135154}; //biceps lowpass 5 Hz, Fsample = 500Hz
Hooglugt 40:7e93c2f1c1e9 23 float bilowpass_states[4];
Hooglugt 40:7e93c2f1c1e9 24
Hooglugt 40:7e93c2f1c1e9 25 arm_biquad_casd_df1_inst_f32 bihighpass;
Hooglugt 40:7e93c2f1c1e9 26 float bihighpass_const[] = {0.97803048, -1.95606096, 0.97803048, 1.95557824 , -0.95654368}; //biceps highpass 0.5 Hz, Fsample = 500Hz
Hooglugt 40:7e93c2f1c1e9 27 float bihighpass_states[4];
Hooglugt 40:7e93c2f1c1e9 28
Hooglugt 40:7e93c2f1c1e9 29 arm_biquad_casd_df1_inst_f32 trilowpass;
Hooglugt 40:7e93c2f1c1e9 30 float trilowpass_const[] = {0.02008337 , 0.04016673 , 0.02008337 , 1.56101808 , -0.64135154}; //biceps lowpass 5 Hz, Fsample = 500Hz
Hooglugt 40:7e93c2f1c1e9 31 float trilowpass_states[4];
Hooglugt 40:7e93c2f1c1e9 32
Hooglugt 40:7e93c2f1c1e9 33 arm_biquad_casd_df1_inst_f32 trihighpass;
Hooglugt 40:7e93c2f1c1e9 34 float trihighpass_const[] = {0.97803048, -1.95606096, 0.97803048, 1.95557824 , -0.95654368}; //biceps highpass 0.5 Hz, Fsample = 500Hz
Hooglugt 40:7e93c2f1c1e9 35 float trihighpass_states[4];
Hooglugt 40:7e93c2f1c1e9 36
Hooglugt 29:7523e4a8e000 37 PwmOut red(LED_RED);
Hooglugt 23:8d9a623dd713 38 PwmOut green(LED_GREEN);
Hooglugt 23:8d9a623dd713 39 PwmOut blue(LED_BLUE);
Hooglugt 29:7523e4a8e000 40
Hooglugt 40:7e93c2f1c1e9 41 uint8_t direction = 0;
Hooglugt 40:7e93c2f1c1e9 42 uint8_t force = 0;
Hooglugt 23:8d9a623dd713 43
Hooglugt 39:0328b1f16a5a 44 void looper()
Hooglugt 23:8d9a623dd713 45 {
Hooglugt 39:0328b1f16a5a 46 //put raw emg value of biceps and triceps in emg_bifloat and emg_trifloat, respectively
Hooglugt 40:7e93c2f1c1e9 47 float emg_bifloat; //Float voor EMG-waarde biceps
Hooglugt 40:7e93c2f1c1e9 48 float emg_trifloat; //Float voor EMG-waarde triceps
Hooglugt 38:277ba1c0693c 49 emg_bifloat = emg0.read(); // read float value (0..1 = 0..3.3V) biceps
Hooglugt 38:277ba1c0693c 50 emg_trifloat = emg1.read(); // read float value (0..1 = 0..3.3V) triceps
Hooglugt 40:7e93c2f1c1e9 51
Hooglugt 40:7e93c2f1c1e9 52 //process emg biceps
Hooglugt 40:7e93c2f1c1e9 53 arm_biquad_cascade_df1_f32(&bihighpass, &emg_bifloat, &filemg_bifloat, 1 );
Hooglugt 40:7e93c2f1c1e9 54 filemg_bifloat = fabs(filemg_bifloat);
Hooglugt 40:7e93c2f1c1e9 55 arm_biquad_cascade_df1_f32(&bilowpass, &filemg_bifloat, &filemg_bifloat, 1 );
Hooglugt 40:7e93c2f1c1e9 56
Hooglugt 40:7e93c2f1c1e9 57 //process emg triceps
Hooglugt 40:7e93c2f1c1e9 58 arm_biquad_cascade_df1_f32(&trihighpass, &emg_trifloat, &filemg_trifloat, 1 );
Hooglugt 40:7e93c2f1c1e9 59 filemg_trifloat = fabs(filemg_trifloat);
Hooglugt 40:7e93c2f1c1e9 60 arm_biquad_cascade_df1_f32(&trilowpass, &filemg_trifloat, &filemg_trifloat, 1 );
Hooglugt 29:7523e4a8e000 61
Hooglugt 31:d8eaf0ce8517 62 /*send value to PC. Line below is used to prevent buffer overrun */
Hooglugt 30:5e5098b0cca6 63 if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) { //VRAAG: praktisch nut hiervan? print emg value wanneer buffercount groter dan 30 is
Hooglugt 30:5e5098b0cca6 64 //pc.printf("%u\n",emg_bivalue);
Hooglugt 29:7523e4a8e000 65 }
Hooglugt 36:af949aaaba01 66
Hooglugt 34:e166187e62ce 67 /* EMG-singaal van biceps en triceps worden hier gefilterd*/
Hooglugt 36:af949aaaba01 68
Hooglugt 40:7e93c2f1c1e9 69 scope.set(0,filemg_bifloat);
Hooglugt 40:7e93c2f1c1e9 70 scope.set(1,filemg_trifloat);
Hooglugt 23:8d9a623dd713 71 scope.send();
Hooglugt 23:8d9a623dd713 72 }
Hooglugt 23:8d9a623dd713 73
Hooglugt 34:e166187e62ce 74 /*
Hooglugt 32:aaf01b1bf05d 75 void resetlooper() // VRAAG: wat gebeurt er wanneer en resetlooper en looper tegelijkertijd gecalled worden?!
Hooglugt 31:d8eaf0ce8517 76 {
Hooglugt 30:5e5098b0cca6 77 if(emg_trifloat.read()>0.8 && direction != 0) { //dit is alleen mogelijk wanneer directionchoice is gemaakt
Hooglugt 30:5e5098b0cca6 78 direction = 0;
Hooglugt 30:5e5098b0cca6 79 force = 0; // WEGHALEN, wanneer in uiteindelijke script na force keuzen niet meer gereset kan worden (voor nu wel handig)
Hooglugt 31:d8eaf0ce8517 80 pc.printf("reset ");
Hooglugt 31:d8eaf0ce8517 81 }
Hooglugt 30:5e5098b0cca6 82 }
Hooglugt 35:c3182df00ec8 83 CONSTANTE RESETS DOOR BEWEGINGSARTEFACTEN */
Hooglugt 31:d8eaf0ce8517 84
Hooglugt 23:8d9a623dd713 85 int main()
Hooglugt 23:8d9a623dd713 86 {
Hooglugt 25:ec41b972b250 87 pc.baud(115200); //baudrate instellen
Hooglugt 40:7e93c2f1c1e9 88 log_timer.attach(looper, 0.002); // The looper() function will be called every 0.002 seconds (with the ticker object)
Hooglugt 40:7e93c2f1c1e9 89 //set up filters triceps
Hooglugt 40:7e93c2f1c1e9 90 arm_biquad_cascade_df1_init_f32(&bilowpass,1 , bilowpass_const, bilowpass_states);
Hooglugt 40:7e93c2f1c1e9 91 arm_biquad_cascade_df1_init_f32(&bihighpass,1 ,bihighpass_const,bihighpass_states);
Hooglugt 40:7e93c2f1c1e9 92 //set up filters triceps
Hooglugt 40:7e93c2f1c1e9 93 arm_biquad_cascade_df1_init_f32(&trilowpass,1 , trilowpass_const, trilowpass_states);
Hooglugt 40:7e93c2f1c1e9 94 arm_biquad_cascade_df1_init_f32(&trihighpass,1 ,trihighpass_const,trihighpass_states);
Hooglugt 40:7e93c2f1c1e9 95
Hooglugt 34:e166187e62ce 96 // reset_timer.attach(resetlooper, 0.1); //
Hooglugt 32:aaf01b1bf05d 97
Hooglugt 37:2d248e64b745 98 goto directionchoice; // goes to first while(1) for the deciding the direction
Hooglugt 37:2d248e64b745 99
Hooglugt 37:2d248e64b745 100 while(1) { //Loop keuze DIRECTION
Hooglugt 37:2d248e64b745 101 directionchoice:
Hooglugt 23:8d9a623dd713 102 for(int i=1; i<4; i++) {
Hooglugt 23:8d9a623dd713 103 if(i==1) { //red
Hooglugt 23:8d9a623dd713 104 red=0;
Hooglugt 23:8d9a623dd713 105 green=1;
Hooglugt 23:8d9a623dd713 106 blue=1;
Hooglugt 37:2d248e64b745 107 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 108 if(filemg_bifloat>0.8) { // 0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 25:ec41b972b250 109 direction = 1;
Hooglugt 37:2d248e64b745 110 red=1;
Hooglugt 37:2d248e64b745 111 green = 0;
Hooglugt 36:af949aaaba01 112 blue = 0;
Hooglugt 37:2d248e64b745 113 pc.printf("links ");
Hooglugt 37:2d248e64b745 114 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 37:2d248e64b745 115 goto forcechoice; // goes to second while(1) for the deciding the force
Hooglugt 23:8d9a623dd713 116 } else {
Hooglugt 23:8d9a623dd713 117 wait(0.1);
Hooglugt 23:8d9a623dd713 118 }
Hooglugt 23:8d9a623dd713 119 }
Hooglugt 23:8d9a623dd713 120 }
Hooglugt 23:8d9a623dd713 121 if(i==2) { //green
Hooglugt 23:8d9a623dd713 122 red =1;
Hooglugt 23:8d9a623dd713 123 green=0;
Hooglugt 23:8d9a623dd713 124 blue=1;
Hooglugt 37:2d248e64b745 125 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 126 if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 25:ec41b972b250 127 direction = 2;
Hooglugt 37:2d248e64b745 128 red=0;
Hooglugt 29:7523e4a8e000 129 green = 1;
Hooglugt 37:2d248e64b745 130 blue = 0;
Hooglugt 37:2d248e64b745 131 pc.printf("mid ");
Hooglugt 37:2d248e64b745 132 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 23:8d9a623dd713 133 goto forcechoice;
Hooglugt 23:8d9a623dd713 134 } else {
Hooglugt 23:8d9a623dd713 135 wait(0.1);
Hooglugt 23:8d9a623dd713 136 }
Hooglugt 23:8d9a623dd713 137 }
Hooglugt 23:8d9a623dd713 138 }
Hooglugt 23:8d9a623dd713 139 if(i==3) { //blue
Hooglugt 23:8d9a623dd713 140 red=1;
Hooglugt 23:8d9a623dd713 141 green=1;
Hooglugt 23:8d9a623dd713 142 blue=0;
Hooglugt 37:2d248e64b745 143 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 144 if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 23:8d9a623dd713 145 direction = 3;
Hooglugt 37:2d248e64b745 146 red=0;
Hooglugt 29:7523e4a8e000 147 green = 0;
Hooglugt 37:2d248e64b745 148 blue = 1;
Hooglugt 37:2d248e64b745 149 pc.printf("rechts ");
Hooglugt 37:2d248e64b745 150 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 23:8d9a623dd713 151 goto forcechoice;
Hooglugt 23:8d9a623dd713 152 } else {
Hooglugt 23:8d9a623dd713 153 wait(0.1);
Hooglugt 23:8d9a623dd713 154 }
Hooglugt 23:8d9a623dd713 155 }
Hooglugt 23:8d9a623dd713 156 }
Hooglugt 25:ec41b972b250 157 }
Hooglugt 23:8d9a623dd713 158 }
Hooglugt 37:2d248e64b745 159 while(1) { //Loop keuze FORCE
Hooglugt 25:ec41b972b250 160 forcechoice:
Hooglugt 37:2d248e64b745 161 for(int j=1; j<4; j++) {
Hooglugt 37:2d248e64b745 162 if(j==1) { //red
Hooglugt 37:2d248e64b745 163 red=0;
Hooglugt 37:2d248e64b745 164 green=1;
Hooglugt 37:2d248e64b745 165 blue=1;
Hooglugt 37:2d248e64b745 166 if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is)
Hooglugt 37:2d248e64b745 167 goto directionchoice;
Hooglugt 37:2d248e64b745 168 }
Hooglugt 37:2d248e64b745 169 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 170 if(filemg_bifloat>0.8) { // 0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 37:2d248e64b745 171 force = 1;
Hooglugt 37:2d248e64b745 172 red=1;
Hooglugt 37:2d248e64b745 173 green = 0;
Hooglugt 37:2d248e64b745 174 blue = 0;
Hooglugt 37:2d248e64b745 175 pc.printf("zwak ");
Hooglugt 37:2d248e64b745 176 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 37:2d248e64b745 177 goto choicesmade;
Hooglugt 37:2d248e64b745 178 } else {
Hooglugt 37:2d248e64b745 179 wait(0.1);
Hooglugt 23:8d9a623dd713 180 }
Hooglugt 23:8d9a623dd713 181 }
Hooglugt 37:2d248e64b745 182 }
Hooglugt 37:2d248e64b745 183 if(j==2) { //green
Hooglugt 37:2d248e64b745 184 red =1;
Hooglugt 37:2d248e64b745 185 green=0;
Hooglugt 37:2d248e64b745 186 blue=1;
Hooglugt 37:2d248e64b745 187 if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is)
Hooglugt 37:2d248e64b745 188 goto directionchoice;
Hooglugt 37:2d248e64b745 189 }
Hooglugt 37:2d248e64b745 190 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 191 if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 37:2d248e64b745 192 force = 2;
Hooglugt 37:2d248e64b745 193 red=0;
Hooglugt 37:2d248e64b745 194 green = 1;
Hooglugt 37:2d248e64b745 195 blue = 0;
Hooglugt 37:2d248e64b745 196 pc.printf("normaal ");
Hooglugt 37:2d248e64b745 197 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 37:2d248e64b745 198 goto choicesmade;
Hooglugt 37:2d248e64b745 199 } else {
Hooglugt 37:2d248e64b745 200 wait(0.1);
Hooglugt 23:8d9a623dd713 201 }
Hooglugt 23:8d9a623dd713 202 }
Hooglugt 37:2d248e64b745 203 }
Hooglugt 37:2d248e64b745 204 if(j==3) { //blue
Hooglugt 37:2d248e64b745 205 red=1;
Hooglugt 37:2d248e64b745 206 green=1;
Hooglugt 37:2d248e64b745 207 blue=0;
Hooglugt 37:2d248e64b745 208 if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is)
Hooglugt 37:2d248e64b745 209 goto directionchoice;
Hooglugt 37:2d248e64b745 210 }
Hooglugt 37:2d248e64b745 211 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 40:7e93c2f1c1e9 212 if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter
Hooglugt 37:2d248e64b745 213 force = 3;
Hooglugt 37:2d248e64b745 214 red=0;
Hooglugt 37:2d248e64b745 215 green = 0;
Hooglugt 37:2d248e64b745 216 blue = 1;
Hooglugt 37:2d248e64b745 217 pc.printf("sterk ");
Hooglugt 37:2d248e64b745 218 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 37:2d248e64b745 219 goto choicesmade;
Hooglugt 37:2d248e64b745 220 } else {
Hooglugt 37:2d248e64b745 221 wait(0.1);
Hooglugt 23:8d9a623dd713 222 }
Hooglugt 23:8d9a623dd713 223 }
Hooglugt 23:8d9a623dd713 224 }
Hooglugt 23:8d9a623dd713 225 }
Hooglugt 37:2d248e64b745 226 }
Hooglugt 37:2d248e64b745 227 choicesmade:
Hooglugt 33:a78ec776dfd5 228
Hooglugt 40:7e93c2f1c1e9 229 red = 0;
Hooglugt 40:7e93c2f1c1e9 230 green = 0;
Hooglugt 40:7e93c2f1c1e9 231 blue = 0; // wit lampje
Hooglugt 40:7e93c2f1c1e9 232
Hooglugt 37:2d248e64b745 233 /* Vanaf hier komt de aansturing van de motor (inclusief de controller)*/
Hooglugt 37:2d248e64b745 234
Hooglugt 33:a78ec776dfd5 235
Hooglugt 25:ec41b972b250 236 if(direction == 1 && force == 1) { // links zwak
Hooglugt 39:0328b1f16a5a 237 pc.printf("links zwak ");
Hooglugt 40:7e93c2f1c1e9 238 wait(2);
Hooglugt 25:ec41b972b250 239 }
Hooglugt 25:ec41b972b250 240 if(direction == 1 && force == 2) { // links normaal
Hooglugt 39:0328b1f16a5a 241 pc.printf("links normaal ");
Hooglugt 25:ec41b972b250 242 }
Hooglugt 25:ec41b972b250 243 if(direction == 1 && force == 3) { // links sterk
Hooglugt 39:0328b1f16a5a 244 pc.printf("links sterk ");
Hooglugt 25:ec41b972b250 245 }
Hooglugt 25:ec41b972b250 246 if(direction == 2 && force == 1) { // mid zwak
Hooglugt 39:0328b1f16a5a 247 pc.printf("mid zwak ");
Hooglugt 25:ec41b972b250 248 }
Hooglugt 25:ec41b972b250 249 if(direction == 2 && force == 2) { // mid normaal
Hooglugt 39:0328b1f16a5a 250 pc.printf("mid normaal ");
Hooglugt 25:ec41b972b250 251 }
Hooglugt 25:ec41b972b250 252 if(direction == 2 && force == 3) { // mid sterk
Hooglugt 39:0328b1f16a5a 253 pc.printf("mid sterk ");
Hooglugt 25:ec41b972b250 254 }
Hooglugt 25:ec41b972b250 255 if(direction == 3 && force == 1) { // rechts zwak
Hooglugt 39:0328b1f16a5a 256 pc.printf("rechts zwak ");
Hooglugt 25:ec41b972b250 257 }
Hooglugt 25:ec41b972b250 258 if(direction == 3 && force == 2) { // rechts normaal
Hooglugt 39:0328b1f16a5a 259 pc.printf("rechts normaal ");
Hooglugt 25:ec41b972b250 260 }
Hooglugt 25:ec41b972b250 261 if(direction == 3 && force == 3) { // rechts sterk
Hooglugt 39:0328b1f16a5a 262 pc.printf("rechts sterk ");
Hooglugt 25:ec41b972b250 263 }
Hooglugt 33:a78ec776dfd5 264 if(direction == 0 || force == 0) { // wanneer de triceps in de korte tijd is aangespannen nadat beide keuzes zijn gemaakt
Hooglugt 39:0328b1f16a5a 265 pc.printf("error ");
Hooglugt 33:a78ec776dfd5 266 // mogelijkheid om een goto te maken naar directionchoice (opzich wel handig)
Hooglugt 32:aaf01b1bf05d 267 }
Hooglugt 40:7e93c2f1c1e9 268
Hooglugt 25:ec41b972b250 269 }