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

Committer:
irisl
Date:
Tue Oct 28 09:49:44 2014 +0000
Revision:
15:eb6498bb7ca0
Parent:
14:257026c95f22
Child:
16:d65458b85734
Functie ingebouwd om t gemiddelde te nemen in de main, wordt nu niet gebruikt

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 12:9e6e49af9304 117
irisl 12:9e6e49af9304 118
jessekaiser 0:db396b9f4b4c 119 void BlinkRed(int n)
jessekaiser 0:db396b9f4b4c 120 {
jessekaiser 0:db396b9f4b4c 121 for (int i=0; i<n; i++) {
jessekaiser 0:db396b9f4b4c 122 myled1 = 1;
jessekaiser 0:db396b9f4b4c 123 myled2 = 1;
jessekaiser 0:db396b9f4b4c 124 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 125 wait(0.1);
jessekaiser 0:db396b9f4b4c 126 myled1 = 0;
jessekaiser 0:db396b9f4b4c 127 myled2 = 1;
jessekaiser 0:db396b9f4b4c 128 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 129 wait(0.1);
jessekaiser 0:db396b9f4b4c 130 }
jessekaiser 0:db396b9f4b4c 131 }
jessekaiser 0:db396b9f4b4c 132
jessekaiser 3:0895fa0a6ca4 133 void BlinkGreen ()
jessekaiser 3:0895fa0a6ca4 134 {
jessekaiser 3:0895fa0a6ca4 135
jessekaiser 3:0895fa0a6ca4 136 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 137 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 138 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 139 wait(0.1);
jessekaiser 3:0895fa0a6ca4 140 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 141 myled2 = 0;
jessekaiser 3:0895fa0a6ca4 142 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 143 wait(0.1);
jessekaiser 3:0895fa0a6ca4 144 }
jessekaiser 3:0895fa0a6ca4 145
jessekaiser 3:0895fa0a6ca4 146
jessekaiser 3:0895fa0a6ca4 147 void BlinkBlue(int n)
jessekaiser 2:39e1bde54e73 148 {
jessekaiser 2:39e1bde54e73 149 for (int i=0; i<n; i++) {
jessekaiser 2:39e1bde54e73 150 myled1 = 1;
jessekaiser 2:39e1bde54e73 151 myled2 = 1;
jessekaiser 2:39e1bde54e73 152 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 153 wait(0.1);
jessekaiser 2:39e1bde54e73 154 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 155 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 156 myled3 = 0;
jessekaiser 3:0895fa0a6ca4 157 wait(0.1);
jessekaiser 2:39e1bde54e73 158 }
jessekaiser 2:39e1bde54e73 159 }
jessekaiser 2:39e1bde54e73 160
irisl 13:493a953a2a85 161 void ShineGreen ()
irisl 13:493a953a2a85 162 {
irisl 13:493a953a2a85 163 myled1 = 1;
irisl 13:493a953a2a85 164 myled2 = 0;
irisl 13:493a953a2a85 165 myled3 = 1;
irisl 13:493a953a2a85 166 }
irisl 13:493a953a2a85 167
irisl 13:493a953a2a85 168 void ShineBlue ()
irisl 13:493a953a2a85 169 {
irisl 13:493a953a2a85 170 myled1 = 1;
irisl 13:493a953a2a85 171 myled2 = 1;
irisl 13:493a953a2a85 172 myled3 = 0;
irisl 13:493a953a2a85 173 }
irisl 13:493a953a2a85 174
irisl 13:493a953a2a85 175 void ShineRed ()
irisl 13:493a953a2a85 176 {
irisl 13:493a953a2a85 177 myled1 = 0;
irisl 13:493a953a2a85 178 myled2 = 1;
irisl 13:493a953a2a85 179 myled3 = 1;
irisl 13:493a953a2a85 180 }
irisl 13:493a953a2a85 181
irisl 15:eb6498bb7ca0 182
irisl 15:eb6498bb7ca0 183 int main()
jessekaiser 1:099b19376f16 184 {
jessekaiser 0:db396b9f4b4c 185 pc.baud(115200);
jessekaiser 1:099b19376f16 186
jessekaiser 0:db396b9f4b4c 187 Ticker log_timer;
jessekaiser 1:099b19376f16 188 //set up filters. Use external array for constants
irisl 9:f7ec578a17c0 189 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
irisl 9:f7ec578a17c0 190 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
irisl 9:f7ec578a17c0 191 arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
irisl 9:f7ec578a17c0 192 arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
jessekaiser 0:db396b9f4b4c 193 /**Here you attach the 'void looper(void)' function to the Ticker object
jessekaiser 0:db396b9f4b4c 194 * The looper() function will be called every 0.01 seconds.
jessekaiser 0:db396b9f4b4c 195 * Please mind that the parentheses after looper are omitted when using attach.
jessekaiser 0:db396b9f4b4c 196 */
jessekaiser 0:db396b9f4b4c 197 log_timer.attach(looper, 0.001);
jessekaiser 1:099b19376f16 198 while(1) { //Loop
jessekaiser 1:099b19376f16 199 /*Empty!*/
jessekaiser 1:099b19376f16 200 /*Everything is handled by the interrupt routine now!*/
jessekaiser 6:8f4138a811e0 201
irisl 13:493a953a2a85 202 while(1) {
irisl 13:493a953a2a85 203 pc.printf("Span de biceps aan om het instellen te starten");
irisl 13:493a953a2a85 204 do {
irisl 13:493a953a2a85 205 BlinkGreen();
irisl 15:eb6498bb7ca0 206 } while(filtered_average_bi < 0.02 && filtered_average_del <0.015); // In rust, geen meting
irisl 15:eb6498bb7ca0 207 if (filtered_average_bi > 0.02 && filtered_average_del < 0.015)
irisl 15:eb6498bb7ca0 208 {
irisl 15:eb6498bb7ca0 209 ShineRed();
irisl 14:257026c95f22 210 }
irisl 15:eb6498bb7ca0 211 if (filtered_average_bi < 0.02 && filtered_average_del > 0.015)
irisl 15:eb6498bb7ca0 212 {
irisl 15:eb6498bb7ca0 213 ShineBlue();
irisl 15:eb6498bb7ca0 214 }
irisl 15:eb6498bb7ca0 215 else if (filtered_average_bi > 0.02 && filtered_average_del > 0.015)
irisl 15:eb6498bb7ca0 216
irisl 15:eb6498bb7ca0 217 {
irisl 14:257026c95f22 218 ShineGreen();
irisl 14:257026c95f22 219 }
jessekaiser 0:db396b9f4b4c 220
jessekaiser 6:8f4138a811e0 221
irisl 14:257026c95f22 222 }
irisl 14:257026c95f22 223 }
jessekaiser 0:db396b9f4b4c 224 }
irisl 15:eb6498bb7ca0 225
irisl 11:bc24a271e6cf 226
irisl 11:bc24a271e6cf 227