r
Dependencies: Encoder HIDScope TextLCD mbed-dsp mbed
Fork of Main-script_groep7 by
Revision 6:a7379a681adf, committed 2014-10-31
- Comitter:
- lauradeheus
- Date:
- Fri Oct 31 12:36:14 2014 +0000
- Parent:
- 5:4842219cb77c
- Child:
- 7:7e3e183bf063
- Commit message:
- Script motoraansturing (nog geen correcte PID waarden) en emgmeting werkend
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed-dsp.lib | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Oct 31 08:36:46 2014 +0000 +++ b/main.cpp Fri Oct 31 12:36:14 2014 +0000 @@ -16,8 +16,9 @@ #include "TextLCD.h" #include "mbed.h" #include "encoder.h" -//#include "HIDScope.h" +#include "HIDScope.h" #include "PwmOut.h" +#include "arm_math.h" /* #define vaste waarden @@ -53,11 +54,17 @@ DigitalOut LEDGREEN(LED_GREEN); DigitalOut LEDRED(LED_RED); Serial pc(USBTX,USBRX); +HIDScope scope(3); +AnalogIn emg(PTB1); /* definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde */ Ticker statemachine; Ticker screen; +arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz +arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz +arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz +arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz int previous_herhalingen = 0; int current_herhalingen = 0; int PWM2_percentage = 100; @@ -72,6 +79,9 @@ int current_pos_motor1; int EMG = 1; int delta_pos_motor1_puls; +int aantal_pieken; +int doel; +bool aanspan; void clamp(float * in, float min, float max); volatile bool looptimerflag; int16_t gewenste_snelheid = 2; @@ -88,7 +98,19 @@ float Speed_motor1; float Speed_motor1rad; float setpoint = 0; -float prev_setpoint = 0; +float prev_setpoint = 0; +float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203}; +float lowpass_1_states[4]; +float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684}; +float lowpass_2_states[4]; +float highpass_const[] = {0.638945525159022 , -1.277891050318045 , 0.638945525159022 , 1.142980502539901 , -0.412801598096189}; +float highpass_states[4]; +float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362}; +float notch_states[4]; +float emg_filtered; +float emg_max = 0; +float emg_treshhold_laag = 0; +float emg_treshhold_hoog = 0; //HIDScope scope(6); @@ -107,8 +129,58 @@ motor2.setPosition(0); } +void pieken_tellen(){ + if (emg_filtered>=emg_treshhold_hoog) + { + aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is. + } + if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt + { + aanspan=false; + aantal_pieken++; + doel = aantal_pieken-((aantal_pieken/3)*3)+1; + } +} + +void emg_filtering() { + uint16_t emg_value; + float emg_value_f32; + emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) + emg_value_f32 = emg.read(); + + arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 ); + arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 ); + arm_biquad_cascade_df1_f32(¬ch, &emg_filtered, &emg_filtered, 1); + emg_filtered = fabs(emg_filtered); + arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 ); + scope.set(0,emg_value); //uint value + scope.set(1,emg_filtered); //processed float + scope.set(2,doel); + scope.send(); + if(state!=EMG_KALIBRATIE) + { + pieken_tellen(); + } +} + +void emg_max_meting(){ + emg_filtering(); + if (emg_filtered>=emg_max) + { + emg_max=emg_filtered; + } + emg_treshhold_laag = 0.3*emg_max; + emg_treshhold_hoog = 0.7*emg_max; +} + void emg_kalibratie() { - current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; + if(emg_filtered>=0.05){ + current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; + } + if(current_herhalingen<=1000) + { + emg_max_meting(); + } } void meten_richting() { @@ -256,7 +328,7 @@ case EMG_KALIBRATIE: { emg_kalibratie(); - if (current_herhalingen == 100) + if (current_herhalingen >=1000) { state = METEN_RICHTING; current_herhalingen = 0; @@ -346,14 +418,43 @@ lcd.locate(0,1); lcd.printf(" GROEP 7 "); } + else if(state==EMG_KALIBRATIE){ + lcd.cls(); + lcd.locate(0,0); + lcd.printf("Max. aanspannen"); + if(current_herhalingen<=200){ + lcd.locate(0,1); + lcd.printf("nog 5 sec."); + } + else if(current_herhalingen<=400){ + lcd.locate(0,1); + lcd.printf("nog 4 sec."); + } + else if(current_herhalingen<=600){ + lcd.locate(0,1); + lcd.printf("nog 3 sec."); + } + else if(current_herhalingen<=800){ + lcd.locate(0,1); + lcd.printf("nog 2 sec."); + } + else if(current_herhalingen<=1000){ + lcd.locate(0,1); + lcd.printf("nog 1 sec."); + } + } else{ lcd.cls(); lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen. } } -int main() { +int main(){ pc.baud(115200); + arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states); + arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states); + arm_biquad_cascade_df1_init_f32(¬ch,1 , notch_const, notch_states); + arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states); statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds) screen.attach(&screenupdate, 0.2); while(1);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-dsp.lib Fri Oct 31 12:36:14 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/mbed-official/code/mbed-dsp/#7a284390b0ce