the slap
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of The_SLAP_5_1 by
Diff: main.cpp
- Revision:
- 1:96cd4c9c5465
- Parent:
- 0:b3809a8d9af1
- Child:
- 2:3bf615031d7a
--- a/main.cpp Tue Oct 21 13:35:52 2014 +0000 +++ b/main.cpp Wed Oct 22 14:16:53 2014 +0000 @@ -13,16 +13,16 @@ #include "HIDScope.h" #include "arm_math.h" #include "encoder.h" +#include "MODSERIAL.h" -#define M1_PWM PTC8 //blauw -#define M1_DIR PTC9 //groen -#define M2_PWM PTA5 //blauw -#define M2_DIR PTA4 //groen +#define M2_PWM PTC8 //blauw +#define M2_DIR PTC9 //groen +#define M1_PWM PTA5 //kleine motor +#define M1_DIR PTA4 //kleine motor #define TSAMP 0.005 // Sampletijd, 200Hz -#define PI 3.1415926535897 -Encoder motor1(PTD2,PTD0); //geel,wit -Encoder motor2(PTD5,PTA13);//geel,wit +Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor +Encoder motor1(PTD5,PTA13);//geel,wit PwmOut pwm_motor1(M1_PWM); PwmOut pwm_motor2(M2_PWM); DigitalOut motordir2(M2_DIR); @@ -31,76 +31,110 @@ AnalogIn emg1(PTB1); //Triceps HIDScope scope(6); -MODSERIAL pc (USBTX,USBRX,64,1024); -pc.baud(115200); +MODSERIAL pc(USBTX,USBRX,64,1024); + -int const windowsamples = 60; //aantal samples waaruit het window voor MOVAG bestaat +float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs,envelop_emg0; //variable to store value in for biceps +float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1; //variable to store value in for triceps -float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs; //variable to store value in for biceps -float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs; //variable to store value in for triceps -float xpos_max,xpos_min,xneg_max,xneg_min; //kalibratiewaardes - -float emg_biceps [windowsamples]; -float emg_triceps [windowsamples]; +arm_biquad_casd_df1_inst_f32 notch_biceps; +arm_biquad_casd_df1_inst_f32 notch_triceps; +float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch +float notch_biceps_states[4]; //state values +float notch_triceps_states[4]; -void meting() -{ - arm_biquad_casd_df1_inst_f32 notch; - //constants for 50Hz notch - float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; - //state values - float notch_states[4]; - arm_biquad_casd_df1_inst_f32 highpass; - //constants for 20Hz highpass - float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189}; - //state values - float highpass_states[4]; - //constants for 80Hz lowpass - arm_biquad_casd_df1_inst_f32 lowpass; - float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189}; - //state values - float lowpass_states[4]; - +arm_biquad_casd_df1_inst_f32 highpass_biceps; +arm_biquad_casd_df1_inst_f32 highpass_triceps; +//constants for 20Hz highpass +float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189}; +//state values +float highpass_biceps_states[4]; +float highpass_triceps_states[4]; +//constants for 80Hz lowpass +arm_biquad_casd_df1_inst_f32 lowpass_biceps; +arm_biquad_casd_df1_inst_f32 lowpass_triceps; +float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189}; +//state values +float lowpass_biceps_states[4]; +float lowpass_triceps_states[4]; + +arm_biquad_casd_df1_inst_f32 envelop_biceps; +arm_biquad_casd_df1_inst_f32 envelop_triceps; +float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016}; +float envelop_biceps_states[4]; +float envelop_triceps_states[4]; + +enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; +uint8_t state=RUST; + +void emgmeten(){ /*put raw emg value in emg_value*/ emg0_value_f32 = emg0.read(); emg1_value_f32 = emg1.read(); //process emg biceps - arm_biquad_cascade_df1_f32(¬ch, &emg0_value_f32, &filtered_emg0_notch, 1 ); - arm_biquad_cascade_df1_f32(&highpass, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 ); - arm_biquad_cascade_df1_f32(&lowpass, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 ); - filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); - emg_biceps [0]= filtered_emg0_eindsignaal_abs; + arm_biquad_cascade_df1_f32(¬ch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 ); + arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 ); + arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 ); + filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass); //gelijkrichter + arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 ); //process emg triceps - arm_biquad_cascade_df1_f32(¬ch, &emg1_value_f32, &filtered_emg1_notch, 1 ); - arm_biquad_cascade_df1_f32(&highpass, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 ); - arm_biquad_cascade_df1_f32(&lowpass, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 ); - filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); - emg_triceps [0]= filtered_emg1_eindsignaal_abs; - - //Movag - //Variabelen voor berekenen gemiddelden - float avg0,avg1; - avg0=avg1=0; - - //Inhoud van een buffer (=gefilterd signaal) optellen - for(int x=0; x<windowsamples; x++) { - avg0 = avg0 + (emg_biceps[x]); - avg1 = avg1 + (emg_triceps[x]); + arm_biquad_cascade_df1_f32(¬ch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 ); + arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 ); + arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 ); + filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass); //gelijkrichter + arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 ); } - - //Gemiddelde berekenen en relativeren tov maximum voluntary contraction - avg0 = gain_biceps*avg0/windowsamples/biceps_max; - avg1 = gain_triceps*avg1/windowsamples/triceps_max; -} + int main() { - Ticker log_timer; - log_timer.attach(meting, TSAMP); - while(1) //Loop - { - - } -} \ No newline at end of file + while(1) { + pc.baud(38400); //PC baud rate is 38400 bits/seconde + Ticker emg_timer; + emg_timer.attach(emgmeten, TSAMP); + arm_biquad_cascade_df1_init_f32(¬ch_biceps,1 , notch_const, notch_biceps_states); + arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states); + arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states); + arm_biquad_cascade_df1_init_f32(¬ch_triceps,1 , notch_const, notch_triceps_states); + arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states); + arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states); + arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states); + arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states); + switch(state) { + case RUST: { //Aanzetten + + state = KALIBRATIE; + break; + } + + case KALIBRATIE: { + pc.printf("Kalibratie"); + wait(1); + + state = RICHTEN; + break; //Stopt acties in deze case + } + + case RICHTEN: { //Batje richten + pc.printf("Richten"); + wait(1); + state = SLAAN; + break; + } + case SLAAN: { //Balletje slaan + pc.printf("Slaan!"); + wait(1); + state = RICHTEN; + break; + } + + default: { + state = RUST; + } //Accolade einde default + + } //Accolade einde switch + + } //Accolade einde main + } \ No newline at end of file