Iris van Leeuwen / Mbed 2 deprecated Lampje_EMG_Gr6

2 losse EMG signalen van de biceps en deltoid

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed Encoder

Fork of Lampje_EMG_Gr6 by Jesse Kaiser

main.cpp@19:1bd2fc3bce1e, 2014-10-29 (annotated)

Committer:
irisl
Date:
Wed Oct 29 08:59:07 2014 +0000
Revision:
19:1bd2fc3bce1e
Parent:
18:ed2afe6953de
Child:
20:d40b6cba4280
Koningen, dubbele presets werken ook! Shinered --> span biceps --> blinkred --> blinkgreen + meten 1e preset --> preset uitvoeren --> 4 sec wachten --> blinkgreen + 2e preset meten --> preset uitvoeren --> terug naar het begin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jessekaiser 0:db396b9f4b4c 1 #include "mbed.h"
jessekaiser 0:db396b9f4b4c 2 #include "HIDScope.h"
jessekaiser 0:db396b9f4b4c 3 #include "arm_math.h"
jessekaiser 0:db396b9f4b4c 4 #include "MODSERIAL.h"
jessekaiser 0:db396b9f4b4c 5
jessekaiser 0:db396b9f4b4c 6 Serial pc(USBTX, USBRX); // tx, rx
jessekaiser 0:db396b9f4b4c 7 DigitalOut myled1(LED_RED);
jessekaiser 0:db396b9f4b4c 8 DigitalOut myled2(LED_GREEN);
jessekaiser 0:db396b9f4b4c 9 DigitalOut myled3(LED_BLUE);
jessekaiser 1:099b19376f16 10 PwmOut motorsignal(PTD4);
jessekaiser 0:db396b9f4b4c 11
jessekaiser 0:db396b9f4b4c 12 //Define objects
jessekaiser 3:0895fa0a6ca4 13 AnalogIn emg0(PTB1); //Analog input
jessekaiser 2:39e1bde54e73 14 AnalogIn emg1(PTB2); //Analog input
irisl 13:493a953a2a85 15 HIDScope scope(2);
jessekaiser 0:db396b9f4b4c 16
irisl 9:f7ec578a17c0 17 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
irisl 9:f7ec578a17c0 18 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
jessekaiser 2:39e1bde54e73 19 //lowpass filter settings: Fc = 225 Hz, Fs = 500 Hz, Gain = -3 dB
jessekaiser 0:db396b9f4b4c 20 float lowpass_const[] = {0.8005910266528647, 1.6011820533057295, 0.8005910266528647, -1.5610153912536877, -0.6413487153577715};
jessekaiser 0:db396b9f4b4c 21 //state values
irisl 9:f7ec578a17c0 22 float lowpass_biceps_states[4];
irisl 9:f7ec578a17c0 23 float lowpass_deltoid_states[4];
irisl 9:f7ec578a17c0 24 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
irisl 9:f7ec578a17c0 25 arm_biquad_casd_df1_inst_f32 highnotch_deltoid;
jessekaiser 2:39e1bde54e73 26 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB
irisl 9:f7ec578a17c0 27 float highnotch_const[] = {0.956542835577484, -1.913085671154968, 0.956542835577484, 1.911196288237583, -0.914975054072353,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
jessekaiser 0:db396b9f4b4c 28 //state values
irisl 9:f7ec578a17c0 29 float highnotch_biceps_states[8];
irisl 9:f7ec578a17c0 30 float highnotch_deltoid_states[8];
jessekaiser 2:39e1bde54e73 31
jessekaiser 2:39e1bde54e73 32 //De globale variabele voor het gefilterde EMG signaal
jessekaiser 2:39e1bde54e73 33 float filtered_biceps;
jessekaiser 2:39e1bde54e73 34 float filtered_deltoid;
irisl 12:9e6e49af9304 35 float filtered_average_bi;
irisl 12:9e6e49af9304 36 float filtered_average_del;
jessekaiser 0:db396b9f4b4c 37
jessekaiser 0:db396b9f4b4c 38
irisl 12:9e6e49af9304 39 void average_biceps(float filtered_biceps,float *average)
irisl 12:9e6e49af9304 40 {
irisl 12:9e6e49af9304 41 static float total=0;
irisl 12:9e6e49af9304 42 static float number=0;
irisl 12:9e6e49af9304 43 total = total + filtered_biceps;
irisl 12:9e6e49af9304 44 number = number + 1;
irisl 18:ed2afe6953de 45 if ( number == 500) {
irisl 18:ed2afe6953de 46 *average = total/500;
irisl 12:9e6e49af9304 47 total = 0;
irisl 12:9e6e49af9304 48 number = 0;
irisl 12:9e6e49af9304 49 }
irisl 12:9e6e49af9304 50 }
irisl 12:9e6e49af9304 51
irisl 12:9e6e49af9304 52 void average_deltoid(float filtered_input,float *average_output)
irisl 12:9e6e49af9304 53 {
irisl 12:9e6e49af9304 54 static float total=0;
irisl 12:9e6e49af9304 55 static float number=0;
irisl 12:9e6e49af9304 56 total = total + filtered_input;
irisl 12:9e6e49af9304 57 number = number + 1;
irisl 18:ed2afe6953de 58 if ( number == 500) {
irisl 18:ed2afe6953de 59 *average_output = total/500;
irisl 12:9e6e49af9304 60 total = 0;
irisl 12:9e6e49af9304 61 number = 0;
irisl 12:9e6e49af9304 62 }
irisl 12:9e6e49af9304 63 }
irisl 12:9e6e49af9304 64
jessekaiser 0:db396b9f4b4c 65 /** Looper function
jessekaiser 0:db396b9f4b4c 66 * functions used for Ticker and Timeout should be of type void <name>(void)
jessekaiser 0:db396b9f4b4c 67 * i.e. no input arguments, no output arguments.
jessekaiser 0:db396b9f4b4c 68 * if you want to change a variable that you use in other places (for example in main)
jessekaiser 0:db396b9f4b4c 69 * you will have to make that variable global in order to be able to reach it both from
jessekaiser 0:db396b9f4b4c 70 * the function called at interrupt time, and in the main function.
jessekaiser 0:db396b9f4b4c 71 * To make a variable global, define it under the includes.
jessekaiser 0:db396b9f4b4c 72 * variables that are changed in the interrupt routine (written to) should be made
jessekaiser 0:db396b9f4b4c 73 * 'volatile' to let the compiler know that those values may change outside the current context.
jessekaiser 0:db396b9f4b4c 74 * i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value"
jessekaiser 0:db396b9f4b4c 75 * in the example below, the variable is not re-used in the main function, and is thus declared
jessekaiser 0:db396b9f4b4c 76 * local in the looper function only.
jessekaiser 0:db396b9f4b4c 77 **/
jessekaiser 1:099b19376f16 78
jessekaiser 1:099b19376f16 79
jessekaiser 0:db396b9f4b4c 80 void looper()
jessekaiser 0:db396b9f4b4c 81 {
jessekaiser 1:099b19376f16 82 /*variable to store value in*/
jessekaiser 2:39e1bde54e73 83 uint16_t emg_value1;
jessekaiser 3:0895fa0a6ca4 84 uint16_t emg_value2;
jessekaiser 1:099b19376f16 85
jessekaiser 2:39e1bde54e73 86 float emg_value1_f32;
jessekaiser 2:39e1bde54e73 87 float emg_value2_f32;
jessekaiser 0:db396b9f4b4c 88 /*put raw emg value both in red and in emg_value*/
jessekaiser 2:39e1bde54e73 89 emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
jessekaiser 2:39e1bde54e73 90 emg_value1_f32 = emg0.read();
jessekaiser 3:0895fa0a6ca4 91
jessekaiser 2:39e1bde54e73 92 emg_value2 = emg1.read_u16();
jessekaiser 2:39e1bde54e73 93 emg_value2_f32 = emg1.read();
jessekaiser 0:db396b9f4b4c 94
jessekaiser 2:39e1bde54e73 95 //process emg biceps
irisl 9:f7ec578a17c0 96 arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
jessekaiser 2:39e1bde54e73 97 filtered_biceps = fabs(filtered_biceps);
irisl 9:f7ec578a17c0 98 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
irisl 12:9e6e49af9304 99 average_biceps(filtered_biceps,&filtered_average_bi);
jessekaiser 2:39e1bde54e73 100 //process emg deltoid
irisl 9:f7ec578a17c0 101 arm_biquad_cascade_df1_f32(&highnotch_deltoid, &emg_value2_f32, &filtered_deltoid, 1 );
jessekaiser 3:0895fa0a6ca4 102 filtered_deltoid = fabs(filtered_deltoid);
irisl 9:f7ec578a17c0 103 arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
irisl 12:9e6e49af9304 104 average_deltoid(filtered_deltoid, &filtered_average_del);
jessekaiser 1:099b19376f16 105
jessekaiser 0:db396b9f4b4c 106 /*send value to PC. */
irisl 9:f7ec578a17c0 107 //scope.set(0,emg_value1); //Raw EMG signal biceps
irisl 9:f7ec578a17c0 108 //scope.set(1,emg_value2); //Raw EMG signal Deltoid
irisl 13:493a953a2a85 109 //scope.set(0,filtered_biceps); //processed float biceps
irisl 13:493a953a2a85 110 scope.set(0,filtered_average_bi); //processed float deltoid
irisl 14:257026c95f22 111 //scope.set(2,filtered_deltoid); //processed float biceps
irisl 13:493a953a2a85 112 scope.set(1,filtered_average_del); //processed float deltoid
jessekaiser 0:db396b9f4b4c 113 scope.send();
jessekaiser 0:db396b9f4b4c 114
jessekaiser 0:db396b9f4b4c 115 }
jessekaiser 0:db396b9f4b4c 116
irisl 16:d65458b85734 117 // LED AANSTURING
irisl 16:d65458b85734 118
irisl 16:d65458b85734 119 Ticker ledticker;
irisl 12:9e6e49af9304 120
irisl 12:9e6e49af9304 121
jessekaiser 0:db396b9f4b4c 122 void BlinkRed(int n)
jessekaiser 0:db396b9f4b4c 123 {
jessekaiser 0:db396b9f4b4c 124 for (int i=0; i<n; i++) {
jessekaiser 0:db396b9f4b4c 125 myled1 = 1;
jessekaiser 0:db396b9f4b4c 126 myled2 = 1;
jessekaiser 0:db396b9f4b4c 127 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 128 wait(0.1);
jessekaiser 0:db396b9f4b4c 129 myled1 = 0;
jessekaiser 0:db396b9f4b4c 130 myled2 = 1;
jessekaiser 0:db396b9f4b4c 131 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 132 wait(0.1);
jessekaiser 0:db396b9f4b4c 133 }
jessekaiser 0:db396b9f4b4c 134 }
jessekaiser 0:db396b9f4b4c 135
irisl 17:dbbe4e126203 136 void greenblink()
irisl 16:d65458b85734 137 {
irisl 17:dbbe4e126203 138 if(myled2.read())
irisl 17:dbbe4e126203 139 myled2 = 0;
irisl 16:d65458b85734 140 else
irisl 17:dbbe4e126203 141 myled2 = 1;
irisl 16:d65458b85734 142 }
irisl 16:d65458b85734 143
irisl 17:dbbe4e126203 144 void BlinkGreen()
irisl 18:ed2afe6953de 145 {
irisl 18:ed2afe6953de 146 myled1= 1;
irisl 18:ed2afe6953de 147 myled3 =1;
irisl 18:ed2afe6953de 148 ledticker.attach(&greenblink,.5);
irisl 18:ed2afe6953de 149 /* myled1 = 1;
irisl 18:ed2afe6953de 150 myled2 = 1;
irisl 18:ed2afe6953de 151 myled3 = 1;
irisl 18:ed2afe6953de 152 wait(0.1);
irisl 18:ed2afe6953de 153 myled1 = 0;
irisl 18:ed2afe6953de 154 myled2 = 1;
irisl 18:ed2afe6953de 155 myled3 = 1;
irisl 18:ed2afe6953de 156 wait(0.1);*/
irisl 16:d65458b85734 157 }
irisl 16:d65458b85734 158
irisl 18:ed2afe6953de 159 void stopblinkgreen()
irisl 16:d65458b85734 160 {
irisl 16:d65458b85734 161 ledticker.detach();
irisl 16:d65458b85734 162 }
irisl 16:d65458b85734 163
irisl 16:d65458b85734 164
irisl 17:dbbe4e126203 165 void BlinkGreen1 ()
jessekaiser 3:0895fa0a6ca4 166 {
jessekaiser 3:0895fa0a6ca4 167
jessekaiser 3:0895fa0a6ca4 168 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 169 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 170 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 171 wait(0.1);
jessekaiser 3:0895fa0a6ca4 172 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 173 myled2 = 0;
jessekaiser 3:0895fa0a6ca4 174 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 175 wait(0.1);
jessekaiser 3:0895fa0a6ca4 176 }
jessekaiser 3:0895fa0a6ca4 177
jessekaiser 3:0895fa0a6ca4 178
jessekaiser 3:0895fa0a6ca4 179 void BlinkBlue(int n)
jessekaiser 2:39e1bde54e73 180 {
jessekaiser 2:39e1bde54e73 181 for (int i=0; i<n; i++) {
jessekaiser 2:39e1bde54e73 182 myled1 = 1;
jessekaiser 2:39e1bde54e73 183 myled2 = 1;
jessekaiser 2:39e1bde54e73 184 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 185 wait(0.1);
jessekaiser 2:39e1bde54e73 186 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 187 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 188 myled3 = 0;
jessekaiser 3:0895fa0a6ca4 189 wait(0.1);
jessekaiser 2:39e1bde54e73 190 }
jessekaiser 2:39e1bde54e73 191 }
jessekaiser 2:39e1bde54e73 192
irisl 13:493a953a2a85 193 void ShineGreen ()
irisl 13:493a953a2a85 194 {
irisl 13:493a953a2a85 195 myled1 = 1;
irisl 13:493a953a2a85 196 myled2 = 0;
irisl 13:493a953a2a85 197 myled3 = 1;
irisl 13:493a953a2a85 198 }
irisl 13:493a953a2a85 199
irisl 13:493a953a2a85 200 void ShineBlue ()
irisl 13:493a953a2a85 201 {
irisl 13:493a953a2a85 202 myled1 = 1;
irisl 13:493a953a2a85 203 myled2 = 1;
irisl 13:493a953a2a85 204 myled3 = 0;
irisl 13:493a953a2a85 205 }
irisl 13:493a953a2a85 206
irisl 13:493a953a2a85 207 void ShineRed ()
irisl 13:493a953a2a85 208 {
irisl 13:493a953a2a85 209 myled1 = 0;
irisl 13:493a953a2a85 210 myled2 = 1;
irisl 13:493a953a2a85 211 myled3 = 1;
irisl 13:493a953a2a85 212 }
irisl 13:493a953a2a85 213
irisl 15:eb6498bb7ca0 214
irisl 16:d65458b85734 215 int main()
jessekaiser 1:099b19376f16 216 {
jessekaiser 0:db396b9f4b4c 217 pc.baud(115200);
jessekaiser 1:099b19376f16 218
jessekaiser 0:db396b9f4b4c 219 Ticker log_timer;
jessekaiser 1:099b19376f16 220 //set up filters. Use external array for constants
irisl 9:f7ec578a17c0 221 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
irisl 9:f7ec578a17c0 222 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
irisl 9:f7ec578a17c0 223 arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
irisl 9:f7ec578a17c0 224 arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
jessekaiser 0:db396b9f4b4c 225 /**Here you attach the 'void looper(void)' function to the Ticker object
jessekaiser 0:db396b9f4b4c 226 * The looper() function will be called every 0.01 seconds.
jessekaiser 0:db396b9f4b4c 227 * Please mind that the parentheses after looper are omitted when using attach.
jessekaiser 0:db396b9f4b4c 228 */
jessekaiser 0:db396b9f4b4c 229 log_timer.attach(looper, 0.001);
jessekaiser 1:099b19376f16 230 while(1) { //Loop
jessekaiser 1:099b19376f16 231 /*Empty!*/
jessekaiser 1:099b19376f16 232 /*Everything is handled by the interrupt routine now!*/
jessekaiser 6:8f4138a811e0 233
irisl 13:493a953a2a85 234 while(1) {
irisl 18:ed2afe6953de 235 pc.printf("Span de biceps aan om het instellen te starten.\n");
irisl 13:493a953a2a85 236 do {
irisl 16:d65458b85734 237 ShineRed();
irisl 17:dbbe4e126203 238 } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
irisl 17:dbbe4e126203 239 if (filtered_average_bi > 0.05) {
irisl 18:ed2afe6953de 240 BlinkRed(10);
irisl 18:ed2afe6953de 241 BlinkGreen();
irisl 18:ed2afe6953de 242 while (1) {
irisl 18:ed2afe6953de 243 pc.printf("In de loop.\n");
irisl 18:ed2afe6953de 244 if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) {
irisl 18:ed2afe6953de 245 stopblinkgreen();
irisl 18:ed2afe6953de 246 pc.printf("Shinered.\n");
irisl 18:ed2afe6953de 247 ShineRed();
irisl 18:ed2afe6953de 248 wait (4);
irisl 18:ed2afe6953de 249 break;
irisl 18:ed2afe6953de 250 }
irisl 18:ed2afe6953de 251 if (filtered_average_bi < 0.05 && filtered_average_del > 0.05) {
irisl 18:ed2afe6953de 252 stopblinkgreen();
irisl 18:ed2afe6953de 253 pc.printf("ShineBlue.\n");
irisl 18:ed2afe6953de 254 ShineBlue();
irisl 18:ed2afe6953de 255 wait(4);
irisl 18:ed2afe6953de 256 break;
irisl 18:ed2afe6953de 257 } else if (filtered_average_bi > 0.05 && filtered_average_del > 0.05)
irisl 18:ed2afe6953de 258
irisl 18:ed2afe6953de 259 {
irisl 18:ed2afe6953de 260 stopblinkgreen();
irisl 18:ed2afe6953de 261 pc.printf("ShineGreen.\n");
irisl 18:ed2afe6953de 262 ShineGreen();
irisl 18:ed2afe6953de 263 wait (4);
irisl 18:ed2afe6953de 264 break;
irisl 18:ed2afe6953de 265 }
irisl 14:257026c95f22 266 }
irisl 19:1bd2fc3bce1e 267 BlinkGreen();
irisl 19:1bd2fc3bce1e 268 while (1) {
irisl 19:1bd2fc3bce1e 269 pc.printf("In de loop.\n");
irisl 19:1bd2fc3bce1e 270 if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) {
irisl 19:1bd2fc3bce1e 271 stopblinkgreen();
irisl 19:1bd2fc3bce1e 272 pc.printf("Shinered.\n");
irisl 19:1bd2fc3bce1e 273 ShineRed();
irisl 19:1bd2fc3bce1e 274 wait (4);
irisl 19:1bd2fc3bce1e 275 break;
irisl 19:1bd2fc3bce1e 276 }
irisl 19:1bd2fc3bce1e 277 if (filtered_average_bi < 0.05 && filtered_average_del > 0.05) {
irisl 19:1bd2fc3bce1e 278 stopblinkgreen();
irisl 19:1bd2fc3bce1e 279 pc.printf("ShineBlue.\n");
irisl 19:1bd2fc3bce1e 280 ShineBlue();
irisl 19:1bd2fc3bce1e 281 wait(4);
irisl 19:1bd2fc3bce1e 282 break;
irisl 19:1bd2fc3bce1e 283 } else if (filtered_average_bi > 0.05 && filtered_average_del > 0.05)
irisl 19:1bd2fc3bce1e 284
irisl 19:1bd2fc3bce1e 285 {
irisl 19:1bd2fc3bce1e 286 stopblinkgreen();
irisl 19:1bd2fc3bce1e 287 pc.printf("ShineGreen.\n");
irisl 19:1bd2fc3bce1e 288 ShineGreen();
irisl 19:1bd2fc3bce1e 289 wait (4);
irisl 19:1bd2fc3bce1e 290 break;
irisl 19:1bd2fc3bce1e 291 }
irisl 19:1bd2fc3bce1e 292 }
jessekaiser 6:8f4138a811e0 293
irisl 14:257026c95f22 294 }
irisl 19:1bd2fc3bce1e 295
jessekaiser 0:db396b9f4b4c 296 }
irisl 16:d65458b85734 297 }
irisl 19:1bd2fc3bce1e 298 }