emg_mk
Dependencies: HIDScope MODSERIAL mbed-dsp mbed
Diff: main.cpp
- Revision:
- 2:a86b09b00008
- Parent:
- 1:6a8b45298e54
- Child:
- 3:69ffa34e4239
diff -r 6a8b45298e54 -r a86b09b00008 main.cpp --- a/main.cpp Tue Oct 21 14:27:50 2014 +0000 +++ b/main.cpp Tue Oct 21 16:14:14 2014 +0000 @@ -3,16 +3,27 @@ #include "MODSERIAL.h" #include "arm_math.h" +MODSERIAL pc(USBTX,USBRX); + HIDScope scope(2);//is dit 4 voor 2 spieren? en hoe zit het met scope.set? -AnalogIn emgB(PTB0); +AnalogIn emgB(PTB0);//biceps +AnalogIn emgT(PTB1); // tricep //*** OBJECTS *** -float (filtered_emgB); +//bicep +float filtered_emgB; float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3 int yB1, yB2, yB3; float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG_B;//moving average objects int snelheidsstand; +//tricep +float filtered_emgT; +float drempelwaardeT1, drempelwaardeT2;//T1=positie 1, T2=positie 2 +int yT1, yT2; +float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, MOVAVG_T;//moving average objects +int positie; + //*** FILTERS *** arm_biquad_casd_df1_inst_f32 lowpass; @@ -25,9 +36,40 @@ float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2} float highpass_states[4]; -//*** CALIBRATIE *** +//*** CALIBRATIE ***//dit moet nog in de main komen! en ik snap dit niet :( +//void Calibratie() +//{ +//pc.printf("Calibratie drempelwaarde Triceps stand 1\n"); +//wait(0.5); +// { +// int i; +// int j=19; + +//for (i=0, i<=j; i++) { +/*variable to store value in*/ +// uint16_t emg_valueT1i_C; +// float emg_value_f32T1i_C; +/*put raw emg value both in red and in emg_value*/ +// emg_valueT1i_C = emgT1i_C.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) +// emg_value_f32T1i_C = emgT1i_C.read(); + +//process emg +// arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T1i_C, &filtered_emgT1i_C, 1 ); +// filtered_emgT1i_C = fabs(filtered_emgT1i_C); +// arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT1i_C, &filtered_emgT1i_C, 1 ); +// } +// } +//} +//****************************** +//Mean Triceps stand 1 +//void MeanTriceps() +//{ // +// float MeanT1=filtered_emgT10_C*0.05+filtered_emgT11_C*0.05+filtered_emgT12_C*0.05+filtered_emgT13_C*0.05+filtered_emgT14_C*0.05+filtered_emgT15_C*0.05+filtered_emgT16_C*0.05+filtered_emgT17_C*0.05+filtered_emgT18_C*0.05+filtered_emgT19_C*0.05+filtered_emgT110_C*0.05+filtered_emgT111_C*0.05+filtered_emgT112_C*0.05+filtered_emgT113_C*0.05+filtered_emgT114_C*0.05+filtered_emgT115_C*0.05+filtered_emgT116_C*0.05+filtered_emgT117_C*0.05+filtered_emgT118_C*0.05+filtered_emgT119_C*0.05; +// +//} +//****************************** //*** BICEP EMG *** void Biceps() @@ -35,25 +77,18 @@ uint16_t emg_valueB; float emg_value_f32B; - //lezen emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) emg_value_f32B = emgB.read(); - //filteren arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 ); - arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 ); - filtered_emgB = fabs(filtered_emgB); - //moving average B0=filtered_emgB; - MOVAVG_B=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1; - B9=B8; B8=B7; B7=B6; @@ -64,59 +99,130 @@ B2=B1; B1=B0; - //naar scherm sturen - scope.set(0,emg_valueB); - scope.set(1,MOVAVG_B); + //naar scherm + scope.set(0,emg_valueB); //ruwe data + scope.set(1,filtered_emgB); //filtered scope.send(); +} // *** TRICEP EMG *** -void Triceps(){ +void Triceps() +{ + uint16_t emg_valueT; + float emg_value_f32T; + + //lezen + emg_valueT = emgT.read_u16(); + emg_value_f32T = emgT.read(); + + //filteren + arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T, &filtered_emgT, 1 ); + filtered_emgT = fabs(filtered_emgT); + arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT, &filtered_emgT, 1 ); + + //moving average + T0=filtered_emgT; + MOVAVG_T=T0*0.1+T1*0.1+T2*0.1+T3*0.1+T4*0.1+T5*0.1+T6*0.1+T7*0.1+T8*0.1+T9*0.1; + T9=T8; + T8=T7; + T7=T6; + T6=T5; + T5=T4; + T4=T3; + T3=T2; + T2=T1; + T1=T0; + + //naar scherm + scope.set(2,emg_valueT); //ruwe data + scope.set(3,filtered_emgT); //filtered + scope.send(); +} // *** MAIN *** - int main() { +int main() +{ + //bepaling van positie met triceps + Ticker log_timerT; + arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states); + arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states); + + log_timerT.attach(Triceps, 0.005); + while(1) {} - Ticker log_timer; - arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states); - arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states); + drempelwaardeT1=4.99; + drempelwaardeT2=7; + if (MOVAVG_T >= drempelwaardeT1) { + yT1=1; + if (MOVAVG_T >= drempelwaardeT1) { + yT2=1; + } else { + yT2=0; + } + } else { + yT1=0; + } - log_timer.attach(Biceps, 0.005); - while(1) {} + positie=yT1+yT2;//INPUT MOTOR 2 - //aansturing - if (filtered_emgB >= drempelwaardeB1) { - yB1=1; - if (filtered_emgB >= drempelwaardeB2) { - yB2=1; - if (filtered_emgB >= drempelwaardeB3) { - yB3=1; - } else { - yB3=0; - } + //controle positie op scherm + if (positie==0) { + pc.printf("Motor 2 gaat naar stand 0\n"); + } else { + if (positie==1) { + pc.printf("Motor 2 gaat naar stand 1\n"); + } else { + if (positie==2) { + pc.printf("Motor 1 beweegt met snelheid 2\n"); + } + } + } + + + //bepaling van snelheidsstand met biceps + Ticker log_timerB; + arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states); + arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states); + + log_timerB.attach(Biceps, 0.005); + while(1) {} + + drempelwaardeB1=4.99; + drempelwaardeB2=6; + drempelwaardeB3=10; + + if (MOVAVG_B >= drempelwaardeB1) { + yB1=1; + if (MOVAVG_B >= drempelwaardeB2) { + yB2=1; + if (MOVAVG_B >= drempelwaardeB3) { + yB3=1; } else { - yB2=0; + yB3=0; } } else { - yB1=0; + yB2=0; } + } else { + yB1=0; + } - //controle - snelheidsstand=yB1+yB2+yB3; + snelheidsstand=yB1+yB2+yB3;//INPUT MOTOR 1 + //controle snelheidsstand op scherm + if (snelheidsstand==0) { + pc.printf("Motor 1 beweegt niet\n"); + } else { if (snelheidsstand==1) { pc.printf("Motor 1 beweegt met snelheid 1\n"); } else { - pc.printf("Motor 1 beweegt niet met snelheid 1\n"); - } - if (snelheidsstand==2) { - pc.printf("Motor 1 beweegt met snelheid 2\n"); - } else { - pc.printf("Motor 1 beweegt niet met snelheid 2\n"); - } - if (snelheidsstand==3) { - pc.printf("Motor 1 beweegt met snelheid 3\n"); - } else { - pc.printf("Motor 1 beweegt niet met snelheid 3\n"); + if (snelheidsstand==2) { + pc.printf("Motor 1 beweegt met snelheid 2\n"); + } else { + if (snelheidsstand==3) { + pc.printf("Motor 1 beweegt met snelheid 3\n"); + } + } } } } -