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@16:d65458b85734, 2014-10-28 (annotated)

Committer:
irisl
Date:
Tue Oct 28 14:02:04 2014 +0000
Revision:
16:d65458b85734
Parent:
15:eb6498bb7ca0
Child:
17:dbbe4e126203
Blinkfunctie;

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 15:eb6498bb7ca0 45 if ( number == 250) {
irisl 15:eb6498bb7ca0 46 *average = total/250;
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 15:eb6498bb7ca0 58 if ( number == 250) {
irisl 15:eb6498bb7ca0 59 *average_output = total/250;
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 16:d65458b85734 136 void redblink()
irisl 16:d65458b85734 137 {
irisl 16:d65458b85734 138 if(myled1.read())
irisl 16:d65458b85734 139 myled1 = 0;
irisl 16:d65458b85734 140 else
irisl 16:d65458b85734 141 myled1 = 1;
irisl 16:d65458b85734 142 }
irisl 16:d65458b85734 143
irisl 16:d65458b85734 144 void BlinkRed()
irisl 16:d65458b85734 145 { ledticker.attach(redblink,.1);
irisl 16:d65458b85734 146 /* myled1 = 1;
irisl 16:d65458b85734 147 myled2 = 1;
irisl 16:d65458b85734 148 myled3 = 1;
irisl 16:d65458b85734 149 wait(0.1);
irisl 16:d65458b85734 150 myled1 = 0;
irisl 16:d65458b85734 151 myled2 = 1;
irisl 16:d65458b85734 152 myled3 = 1;
irisl 16:d65458b85734 153 wait(0.1);*/
irisl 16:d65458b85734 154 }
irisl 16:d65458b85734 155
irisl 16:d65458b85734 156 void stopblinkred()
irisl 16:d65458b85734 157 {
irisl 16:d65458b85734 158 ledticker.detach();
irisl 16:d65458b85734 159 }
irisl 16:d65458b85734 160
irisl 16:d65458b85734 161
jessekaiser 3:0895fa0a6ca4 162 void BlinkGreen ()
jessekaiser 3:0895fa0a6ca4 163 {
jessekaiser 3:0895fa0a6ca4 164
jessekaiser 3:0895fa0a6ca4 165 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 166 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 167 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 168 wait(0.1);
jessekaiser 3:0895fa0a6ca4 169 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 170 myled2 = 0;
jessekaiser 3:0895fa0a6ca4 171 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 172 wait(0.1);
jessekaiser 3:0895fa0a6ca4 173 }
jessekaiser 3:0895fa0a6ca4 174
jessekaiser 3:0895fa0a6ca4 175
jessekaiser 3:0895fa0a6ca4 176 void BlinkBlue(int n)
jessekaiser 2:39e1bde54e73 177 {
jessekaiser 2:39e1bde54e73 178 for (int i=0; i<n; i++) {
jessekaiser 2:39e1bde54e73 179 myled1 = 1;
jessekaiser 2:39e1bde54e73 180 myled2 = 1;
jessekaiser 2:39e1bde54e73 181 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 182 wait(0.1);
jessekaiser 2:39e1bde54e73 183 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 184 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 185 myled3 = 0;
jessekaiser 3:0895fa0a6ca4 186 wait(0.1);
jessekaiser 2:39e1bde54e73 187 }
jessekaiser 2:39e1bde54e73 188 }
jessekaiser 2:39e1bde54e73 189
irisl 13:493a953a2a85 190 void ShineGreen ()
irisl 13:493a953a2a85 191 {
irisl 13:493a953a2a85 192 myled1 = 1;
irisl 13:493a953a2a85 193 myled2 = 0;
irisl 13:493a953a2a85 194 myled3 = 1;
irisl 13:493a953a2a85 195 }
irisl 13:493a953a2a85 196
irisl 13:493a953a2a85 197 void ShineBlue ()
irisl 13:493a953a2a85 198 {
irisl 13:493a953a2a85 199 myled1 = 1;
irisl 13:493a953a2a85 200 myled2 = 1;
irisl 13:493a953a2a85 201 myled3 = 0;
irisl 13:493a953a2a85 202 }
irisl 13:493a953a2a85 203
irisl 13:493a953a2a85 204 void ShineRed ()
irisl 13:493a953a2a85 205 {
irisl 13:493a953a2a85 206 myled1 = 0;
irisl 13:493a953a2a85 207 myled2 = 1;
irisl 13:493a953a2a85 208 myled3 = 1;
irisl 13:493a953a2a85 209 }
irisl 13:493a953a2a85 210
irisl 15:eb6498bb7ca0 211
irisl 16:d65458b85734 212 int main()
jessekaiser 1:099b19376f16 213 {
jessekaiser 0:db396b9f4b4c 214 pc.baud(115200);
jessekaiser 1:099b19376f16 215
jessekaiser 0:db396b9f4b4c 216 Ticker log_timer;
jessekaiser 1:099b19376f16 217 //set up filters. Use external array for constants
irisl 9:f7ec578a17c0 218 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
irisl 9:f7ec578a17c0 219 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
irisl 9:f7ec578a17c0 220 arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
irisl 9:f7ec578a17c0 221 arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
jessekaiser 0:db396b9f4b4c 222 /**Here you attach the 'void looper(void)' function to the Ticker object
jessekaiser 0:db396b9f4b4c 223 * The looper() function will be called every 0.01 seconds.
jessekaiser 0:db396b9f4b4c 224 * Please mind that the parentheses after looper are omitted when using attach.
jessekaiser 0:db396b9f4b4c 225 */
jessekaiser 0:db396b9f4b4c 226 log_timer.attach(looper, 0.001);
jessekaiser 1:099b19376f16 227 while(1) { //Loop
jessekaiser 1:099b19376f16 228 /*Empty!*/
jessekaiser 1:099b19376f16 229 /*Everything is handled by the interrupt routine now!*/
jessekaiser 6:8f4138a811e0 230
irisl 13:493a953a2a85 231 while(1) {
irisl 13:493a953a2a85 232 pc.printf("Span de biceps aan om het instellen te starten");
irisl 13:493a953a2a85 233 do {
irisl 16:d65458b85734 234 ShineRed();
irisl 15:eb6498bb7ca0 235 } while(filtered_average_bi < 0.02 && filtered_average_del <0.015); // In rust, geen meting
irisl 16:d65458b85734 236 if (filtered_average_bi > 0.02) {
irisl 16:d65458b85734 237 BlinkRed(5);
irisl 16:d65458b85734 238 BlinkRed();
irisl 16:d65458b85734 239 if (filtered_average_bi > 0.02 && filtered_average_del < 0.015) {
irisl 16:d65458b85734 240 stopblinkred();
irisl 16:d65458b85734 241 ShineRed();
irisl 16:d65458b85734 242 wait (4);
irisl 14:257026c95f22 243 }
irisl 16:d65458b85734 244 if (filtered_average_bi < 0.02 && filtered_average_del > 0.015) {
irisl 16:d65458b85734 245 stopblinkred();
irisl 16:d65458b85734 246 ShineBlue();
irisl 16:d65458b85734 247 wait(4);
irisl 16:d65458b85734 248 } else if (filtered_average_bi > 0.02 && filtered_average_del > 0.015)
irisl 16:d65458b85734 249
irisl 16:d65458b85734 250 {
irisl 16:d65458b85734 251 stopblinkred();
irisl 16:d65458b85734 252 ShineGreen();
irisl 16:d65458b85734 253 wait (4);
irisl 15:eb6498bb7ca0 254 }
jessekaiser 0:db396b9f4b4c 255
jessekaiser 6:8f4138a811e0 256
irisl 14:257026c95f22 257 }
jessekaiser 0:db396b9f4b4c 258 }
irisl 16:d65458b85734 259 }
irisl 16:d65458b85734 260 }
irisl 11:bc24a271e6cf 261
irisl 11:bc24a271e6cf 262