2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
Diff: main.cpp
- Revision:
- 16:c34c5d9dfe1a
- Parent:
- 15:3ebd0e666a9c
- Child:
- 17:c694a0e63a89
diff -r 3ebd0e666a9c -r c34c5d9dfe1a main.cpp --- a/main.cpp Tue Oct 28 11:11:13 2014 +0000 +++ b/main.cpp Tue Oct 28 18:19:08 2014 +0000 @@ -6,17 +6,18 @@ //Constanten definiëren en waarde geven #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor -#define KP_arm1 0.01 //Factor proprotionele regelaar arm 1 -#define KI_arm1 0 //Factor integraal regelaar arm 1 -#define KD_arm1 0 //Factor afgeleide regelaar arm 1 -#define KP_arm2 0.01 //Factor proprotionele regelaar arm 2 +#define KP_arm1 0.01 //Factor proprotionele regelaar arm 1 +#define KI_arm1 0 //Factor integraal regelaar arm 1 +#define KD_arm1 0 //Factor afgeleide regelaar arm 1 +#define KP_arm2 0.01 //Factor proprotionele regelaar arm 2 #define KI_arm2 0 //Factor integraal regelaar arm 2 -#define KD_arm2 0 //Factor afgeleide regelaar arm 2 +#define KD_arm2 0 //Factor afgeleide regelaar arm 2 #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten #define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft #define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft -//High Pass filter Filtercoëfficiënten +//Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk +//High Pass filter #define A1 1 #define A2 -1.5610 #define A3 0.6414 @@ -24,7 +25,7 @@ #define B2 0.0402 #define B3 0.0201 -//Notch filter Filtercoëfficiënten +//Notch filter #define C1 1 #define C2 -1.1873E-16 #define C3 0.9391 @@ -32,7 +33,7 @@ #define D2 -1.1873E-16 #define D3 0.9695 -//Low pass filter Filtercoëfficiënten +//Low pass filter #define E1 1 #define E2 -1.9645 #define E3 0.9651 @@ -40,34 +41,30 @@ #define F2 3.1030E-4 #define F3 1.5515E-4 - //Pinverdeling en naamgeving variabelen -TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm -MODSERIAL pc(USBTX, USBRX); //PC +TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); //LCD scherm +MODSERIAL pc(USBTX, USBRX); //PC -PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 -DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1 -Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit) -PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 -DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2 -Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit) -AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps -AnalogIn EMG_tri(PTB2); -//Blauw op 3,3 V en groen op GND +PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 +DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1 +Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit) +PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 +DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2 +Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit) +AnalogIn EMG_bi(PTB1); //Uitlezen EMG signaal biceps +AnalogIn EMG_tri(PTB2); //Uitlezen EMG signaal triceps +//Blauw op 3,3 V en groen op GND, voeding beide encoders Ticker ticker_regelaar; //Ticker voor regelaar motor Ticker ticker_EMG; //Ticker voor EMG meten -Timer biceps_kalibratie; -Timer triceps_kalibratie; +Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps +Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps //States definiëren -enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, BEGIN_METEN, B, T}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 +enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, BEGIN_METEN, B, T, BB, BT, TB, TT}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 uint8_t state = RUST; //State is rust aan het begin //Gedefinieerde datatypen en naamgeving en beginwaarden -char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm -char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm - float pwm_to_motor_arm1; //PWM output naar motor arm 1 float pwm_to_motor_arm2; //PWM output naar motor arm 2 float error_arm1_kalibratie; //Error in pulsen arm 1 @@ -79,40 +76,40 @@ float integraal_arm2 = 0; //Integraal actie van regelaar arm 2 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2 float xb; //Gemeten EMG waarde biceps -float xt; +float xt; //Gemeten EMG waarde triceps -arm_biquad_casd_df1_inst_f32 highpass_biceps; -float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3}; -float highpass_biceps_states[4]; +arm_biquad_casd_df1_inst_f32 highpass_biceps; //Highpass_biceps IIR filter in direct form 1 +float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter +float highpass_biceps_states[4]; //Aantal states van het filter, het aantal y(n-x) en x(n-x) -arm_biquad_casd_df1_inst_f32 notch_biceps; -float notch_biceps_const[] = {D1, D2, D3, -C2, -C3}; -float notch_biceps_states[4]; +arm_biquad_casd_df1_inst_f32 notch_biceps; //Notch_biceps IIR filter in direct form 1 +float notch_biceps_const[] = {D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter +float notch_biceps_states[4]; //Aantal states van het filter -arm_biquad_casd_df1_inst_f32 lowpass_biceps; -float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3}; -float lowpass_biceps_states[4]; +arm_biquad_casd_df1_inst_f32 lowpass_biceps; //Lowpass_biceps IIR filter in direct form 1 +float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter +float lowpass_biceps_states[4]; //Aantal states van het filter -float xbf; -float xbfmax = 0; +float xbf; //Gefilterde EMG waarde biceps +float xbfmax = 0; //Maximale gefilterde EMG waarde biceps -arm_biquad_casd_df1_inst_f32 highpass_triceps; -float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3}; -float highpass_triceps_states[4]; +arm_biquad_casd_df1_inst_f32 highpass_triceps; //Highpass_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float +float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter +float highpass_triceps_states[4]; //Aantal states van het filter -arm_biquad_casd_df1_inst_f32 notch_triceps; -float notch_triceps_const[] = {D1, D2, D3, -C2, -C3}; -float notch_triceps_states[4]; +arm_biquad_casd_df1_inst_f32 notch_triceps; //Notch_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float +float notch_triceps_const[] = {D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter +float notch_triceps_states[4]; //Aantal states van het filter -arm_biquad_casd_df1_inst_f32 lowpass_triceps; -float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3}; -float lowpass_triceps_states[4]; +arm_biquad_casd_df1_inst_f32 lowpass_triceps; //Lowpass_biceps IIR filter in direct form 1, waarde wordt opgeslagen in een float +float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter +float lowpass_triceps_states[4]; //Aantal states van het filter -float xtf; -float xtfmax = 0; +float xtf; //Gefilterde EMG waarde triceps +float xtfmax = 0; //Maximale gefilterde EMG waarde triceps -float xbt; -float xtt; +float xbt; //Thresholdwaarde EMG biceps +float xtt; //Thresholdwaarde EMG triceps volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true @@ -128,116 +125,134 @@ void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt { - if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum + if (*in < min) //Als de waarde kleiner is als het minimum wordt de waarde het minimum + { *in = min; } - if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum + if (*in > max) //Als de waarde groter is dan het maximum is de waarde het maximum + { *in = max; - } else { //In alle andere gevallen is de waarde de waarde zelf + } + else //In alle andere gevallen is de waarde de waarde zelf + { *in = *in; } } -void arm1_naar_thuispositie() +void arm1_naar_thuispositie() //Brengt arm 1 naar de beginpositie { - error_arm1_kalibratie = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor - integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar - afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar - pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1; //Output naar motor na PID regelaar - keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) + error_arm1_kalibratie = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor + integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar + afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar + pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar + keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) - if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1 + if (pwm_to_motor_arm1 > 0) //Als PWM is positief, dan richting 1 + { dir_motor_arm1.write(1); - } else { //Anders richting nul + } + else //Anders richting nul + { dir_motor_arm1.write(0); } pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM - pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition()); - pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); + pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen + pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen - if (pwm_to_motor_arm1 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) + if (pwm_to_motor_arm1 == 0) //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) + { state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel - pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie + pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie } } -void arm2_naar_thuispositie() +void arm2_naar_thuispositie() //Brengt arm 2 naar beginpositie { - error_arm2_kalibratie = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor - integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar - afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar - pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2; //Output naar motor na PID regelaar - keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) + error_arm2_kalibratie = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor + integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar + afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar + pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar + keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) - if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1 + if (pwm_to_motor_arm2 > 0) //Als PWM is positief, dan richting 1 + { dir_motor_arm2.write(1); - } else { //Anders richting nul + } + else //Anders richting nul + { dir_motor_arm2.write(0); } pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM - pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition()); - pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2); + pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen + pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2); //Huidige PWM waarde naar pc sturen - if (pwm_to_motor_arm2 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) - state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel - pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie + if (pwm_to_motor_arm2 == 0) //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) + { + state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel + pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie } } -void filter_biceps() +void filter_biceps() //Filtert het EMG signaal van de biceps { - arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1); + arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1); //Highpass filter voor het EMG signaal van de biceps, filtert de xb en schrijft het over in de xbf, 1 EMG waarde filteren per keer - arm_biquad_cascade_df1_f32(¬ch_biceps, &xbf, &xbf, 1); + arm_biquad_cascade_df1_f32(¬ch_biceps, &xbf, &xbf, 1); //Notch filter voor het EMG signaal van de biceps, filtert de xbf uit de highpass en schrijft het opnieuw in de xbf, 1 EMG waarde filteren per keer + + xbf = fabs(xbf); //Rectifier, schrijft de absolute waarde van de xbf uit de notch opnieuw in de xbf, fabs omdat het een float is - xbf = fabs(xbf); + arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1); //Lowpass filter voor het EMG signaal van de biceps, filtert de xbf uit de rectifier en schrijft het opnieuw in de xbf, 1 EMG waarde filteren per keer + + pc.printf("xbf is %f\n\r", xbf); //De gefilterde EMG waarde van de biceps naar pc sturen +} - arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1); - - pc.printf("xbf is %f\n\r", xbf); +void filter_triceps() //Filtert het EMG signaal van de triceps +{ + arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1); //Highpass filter voor het EMG signaal van de triceps, filtert de xt en schrijft het over in de xtf, 1 EMG waarde filteren per keer + + arm_biquad_cascade_df1_f32(¬ch_triceps, &xtf, &xtf, 1); //Notch filter voor het EMG signaal van de triceps, filtert de xtf uit de highpass en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer + xtf = fabs(xtf); //Rectifier, schrijft de absolute waarde van de xtf uit de notch opnieuw in de xtf, fabs omdat het een float is + + arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1); //Lowpass filter voor het EMG signaal van de triceps, filtert de xtf uit de rectifier en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer + + pc.printf("xtf is %f\n\r", xtf); //De gefilterde EMG waarde van de triceps naar pc sturen } -void filter_triceps() +int main() //Main script { - arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1); - - arm_biquad_cascade_df1_f32(¬ch_triceps, &xtf, &xtf, 1); - - xtf = fabs(xtf); - - arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1); - - pc.printf("xtf is %f\n\r", xtf); - -} - -int main() -{ - while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan - //PC baud rate instellen + while(1) //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan + { pc.baud(38400); //PC baud rate is 38400 bits/seconde - switch(state) { //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case + switch(state) //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case + { - case RUST: { //Aanzetten - lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm - lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm - wait(2); //Twee seconden wachten + case RUST: //Aanzetten + { + pc.printf("RUST\n\r"); //Begin RUST naar pc sturen + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" BMT K9 GR. 4 "); //Tekst op LCD scherm + lcd.locate(0,1); //Zet de tekst op de tweede regel + lcd.printf("HOI! IK BEN PIPO"); //Tekst op LCD scherm + wait(2); //Twee seconden wachten + lcd.cls(); //Maak LCD scherm leeg pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten - state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel - break; //Stopt acties in deze case + state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel + break; //Stopt acties in deze case } - case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie - pc.printf("KALIBRATIE_ARM1\n\r"); - wait(1); //Een seconde wachten + case KALIBRATIE_ARM1: //Arm 1 naar thuispositie + { + pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM1 naar pc sturen + wait(1); //Een seconde wachten ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken - while(state == KALIBRATIE_ARM1) { - while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel + while(state == KALIBRATIE_ARM1) + { + while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen @@ -246,39 +261,43 @@ break; //Stopt acties in deze case } - case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie - pc.printf("KALIBRATIE_ARM1\n\r"); - wait(1); //Een seconde wachten + case KALIBRATIE_ARM2: //Arm 2 naar thuispositie + { + pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen + wait(1); //Een seconde wachten //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken - while(state == KALIBRATIE_ARM2) { - while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel + while(state == KALIBRATIE_ARM2) + { + while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen } wait(1); //Een seconde wachten ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer - break; //Stopt acties in deze case + break; //Stopt acties in deze case } - case EMG_KALIBRATIE_BICEPS: { - pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); - - lcd_r1 = " SPAN IN 5 SEC. "; - lcd_r2 = " 2 X BICEPS AAN "; - pc.printf("span biceps aan\n\r"); - pc.printf("EMG biceps %f\n\r",xb); + case EMG_KALIBRATIE_BICEPS: //Kalibratie biceps voor bepalen threshold + { + pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen + + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm + lcd.locate(0,1); //Zet tekst op tweede regel + lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm - arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1, highpass_biceps_const, highpass_biceps_states); - arm_biquad_cascade_df1_init_f32(¬ch_biceps, 1, notch_biceps_const, notch_biceps_states); - arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1, lowpass_biceps_const, lowpass_biceps_states); + arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1, highpass_biceps_const, highpass_biceps_states); //Highpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren + arm_biquad_cascade_df1_init_f32(¬ch_biceps, 1, notch_biceps_const, notch_biceps_states); //Notchfilter biceps met bijbehorende filtercoëfficiënten en states definiëren + arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1, lowpass_biceps_const, lowpass_biceps_states); //Lowpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren - ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); - biceps_kalibratie.start(); + ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken + biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt - while(biceps_kalibratie.read() <= 5) { + while(biceps_kalibratie.read() <= 5) //Zolang de timer nog geen 5 seconden heeft gemeten + { while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan @@ -288,19 +307,23 @@ filter_biceps(); if(int(biceps_kalibratie.read()) == 1) { - lcd_r1 = " 4 "; + lcd.locate(0,0); + lcd.printf(" 4 "); pc.printf("4"); } - if(biceps_kalibratie.read() == 2) { - lcd_r1 = " 3 "; + if(int(biceps_kalibratie.read()) == 2) { + lcd.locate(0,0); + lcd.printf(" 3 "); pc.printf("3"); } - if(biceps_kalibratie.read() == 3) { - lcd_r1 = " 2 "; + if(int(biceps_kalibratie.read()) == 3) { + lcd.locate(0,0); + lcd.printf(" 2 "); pc.printf("2"); } - if(biceps_kalibratie.read() == 4) { - lcd_r1 = " 1 "; + if(int(biceps_kalibratie.read()) == 4) { + lcd.locate(0,0); + lcd.printf(" 1 "); pc.printf("1"); } @@ -317,10 +340,10 @@ case EMG_KALIBRATIE_TRICEPS: { pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); - lcd_r1 = " SPAN IN 5 SEC. "; - lcd_r2 = " 2 X TRICEPS AAN"; - pc.printf("span triceps aan\n\r"); - pc.printf("EMG biceps %f\n\r",xt); + lcd.locate(0,0); + lcd.printf(" SPAN IN 5 SEC. "); + lcd.locate(0,1); + lcd.printf(" 2 X TRICEPS AAN"); arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1, highpass_triceps_const, highpass_triceps_states); arm_biquad_cascade_df1_init_f32(¬ch_triceps, 1, notch_triceps_const, notch_triceps_states); @@ -339,19 +362,24 @@ filter_triceps(); if(triceps_kalibratie.read() == 1) { - lcd_r1 = " 4 "; + lcd.locate(0,0); + lcd.printf(" 4 "); pc.printf("4"); } if(triceps_kalibratie.read() == 2) { - lcd_r1 = " 3 "; + lcd.locate(0,0); + lcd.printf(" 3 "); pc.printf("3"); } if(triceps_kalibratie.read() == 3) { - lcd_r1 = " 2 "; + lcd.locate(0,0); + lcd.printf(" 2 "); pc.printf("2"); } if(triceps_kalibratie.read() == 4) { - lcd_r1 = " 1 "; + lcd.locate(0,0); + lcd.printf(" 1 "); + pc.printf("1"); } } @@ -365,9 +393,11 @@ } case BEGIN_METEN: { - lcd_r1 = " BEGIN "; - pc.printf("KEUZE1\n\r"); - + lcd.locate(0,0); + lcd.printf(" START "); + + pc.printf("START\n\r"); + while(state == BEGIN_METEN) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; @@ -384,14 +414,62 @@ state = T; } } + } + + case B: { + lcd.locate(0,0); + lcd.printf(" B "); + pc.printf("B\n\r"); + while(state == B) { + while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel + regelaar_EMG_flag = false; + xb = EMG_bi.read(); + filter_biceps(); + xt = EMG_tri.read(); + filter_triceps(); + + if(xb >= xbt) { + state = BB; + } + if(xt >= xtt) { + state = BT; + } + } + } + + case T: { + lcd.locate(0,0); + lcd.printf(" T "); + pc.printf("T\n\r"); + + while(state == T) { + while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel + regelaar_EMG_flag = false; + + xb = EMG_bi.read(); + filter_biceps(); + xt = EMG_tri.read(); + filter_triceps(); + + if(xb >= xbt) { + state = TB; + } + if(xt >= xtt) { + state = TT; + } + } + } + + case BB: - default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case - state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan - } + default: + { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case + state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan } + pc.printf("state: %u\n\r",state); } }