emg
Dependencies: HIDScope MODSERIAL mbed-dsp mbed TouchButton
Fork of test by
main.cpp
- Committer:
- s1340735
- Date:
- 2014-10-23
- Revision:
- 10:9319e872c752
- Parent:
- 9:a1890454e5a7
- Child:
- 11:5044290660b0
File content as of revision 10:9319e872c752:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "arm_math.h" MODSERIAL pc(USBTX,USBRX); HIDScope scope(4); AnalogIn emgB(PTB1);//biceps AnalogIn emgT(PTB2); // tricep //*** OBJECTS *** //bicep uint16_t emg_valueB; float emg_value_f32B; 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 uint16_t emg_valueT; float emg_value_f32T; 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 lowpassT; arm_biquad_casd_df1_inst_f32 lowpassB; //constants for 50Hz lowpass float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator float lowpass_states[4]; arm_biquad_casd_df1_inst_f32 highpassT; arm_biquad_casd_df1_inst_f32 highpassB; //constants for 10Hz highpass float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2} float highpass_states[4]; //*** 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; // //} //****************************** // *** TRICEPS en BICEPS EMG *** void Triceps() { //Triceps lezen emg_valueT = emgT.read_u16(); emg_value_f32T = emgT.read(); //Triceps filteren arm_biquad_cascade_df1_f32(&highpassT, &emg_value_f32T, &filtered_emgT, 1 ); arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 ); filtered_emgT = fabs(filtered_emgT); //sturen naar scherm (Realterm) pc.printf("%f\r\n",filtered_emgT);//u=f filtered_emgT is een float //Triceps 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; //sturen naar HID Scope scope.set(0,emg_valueT); //ruwe data scope.set(1,filtered_emgT); //filtered scope.send(); } void Biceps() { //Biceps 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(); //Biceps filteren arm_biquad_cascade_df1_f32(&highpassB, &emg_value_f32B, &filtered_emgB, 1 ); arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 ); filtered_emgB = fabs(filtered_emgB); //sturen naar scherm pc.printf("%f\r\n",filtered_emgB); //Biceps 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; B6=B5; B5=B4; B4=B3; B3=B2; B2=B1; B1=B0; //naar HID Scope scope.set(2,emg_valueB); //ruwe data scope.set(3,filtered_emgB); //filtered scope.send(); } // *** MAIN *** int main() { pc.baud(115200); //bepaling van positie met triceps Ticker log_timerT; arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states); arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states); log_timerT.attach(Triceps, 0.005); wait(10); //log_timerT wordt 2000 keer uitgevoerd log_timerT.detach(); wait(5); Ticker log_timerB; arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states); arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states); log_timerB.attach(Biceps,0.005); wait(10);//log_timerB wordt 2000 keer uitgevoerd log_timerB.detach(); // positie van batje met behulp van Triceps drempelwaardeT1=4; drempelwaardeT2=7; if (MOVAVG_T >= drempelwaardeT1) { yT1=1; if (MOVAVG_T >= drempelwaardeT1) { yT2=1; } else { yT2=0; } } else { yT1=0; } //*** INPUT MOTOR 2 *** positie=yT1+yT2; //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 drempelwaardeB1=4; drempelwaardeB2=6; drempelwaardeB3=10; if (MOVAVG_B >= drempelwaardeB1) { yB1=1; if (MOVAVG_B >= drempelwaardeB2) { yB2=1; if (MOVAVG_B >= drempelwaardeB3) { yB3=1; } else { yB3=0; } } else { yB2=0; } } else { yB1=0; } //*** INPUT MOTOR 1 *** snelheidsstand=yB1+yB2+yB3; //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 { 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"); } } } } }