![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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:
- 17:c694a0e63a89
- Parent:
- 16:c34c5d9dfe1a
- Child:
- 18:d3a7f8b4cb22
diff -r c34c5d9dfe1a -r c694a0e63a89 main.cpp --- a/main.cpp Tue Oct 28 18:19:08 2014 +0000 +++ b/main.cpp Wed Oct 29 11:51:32 2014 +0000 @@ -42,7 +42,7 @@ #define F3 1.5515E-4 //Pinverdeling en naamgeving variabelen -TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); //LCD scherm +TextLCD lcd(PTD1, PTA12, PTB2, PTB3, PTC2, PTD3, TextLCD::LCD16x2); //LCD scherm MODSERIAL pc(USBTX, USBRX); //PC PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 @@ -52,7 +52,7 @@ 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 +AnalogIn EMG_tri(PTB0); //Uitlezen EMG signaal triceps //Blauw op 3,3 V en groen op GND, voeding beide encoders Ticker ticker_regelaar; //Ticker voor regelaar motor @@ -61,7 +61,7 @@ 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, BB, BT, TB, TT}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 +enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT}; //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 @@ -79,31 +79,31 @@ float xt; //Gemeten EMG waarde triceps 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) +float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter +float highpass_biceps_states[8]; //Aantal states van het filter, het aantal y(n-x) en x(n-x) 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 +float notch_biceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter +float notch_biceps_states[8]; //Aantal states van het filter 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 lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter +float lowpass_biceps_states[8]; //Aantal states van het filter float xbf; //Gefilterde EMG waarde biceps float xbfmax = 0; //Maximale gefilterde EMG waarde biceps 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 +float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter +float highpass_triceps_states[8]; //Aantal states van het filter 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 +float notch_triceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter +float notch_triceps_states[8]; //Aantal states van het filter 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 lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter +float lowpass_triceps_states[8]; //Aantal states van het filter float xtf; //Gefilterde EMG waarde triceps float xtfmax = 0; //Maximale gefilterde EMG waarde triceps @@ -196,7 +196,7 @@ void filter_biceps() //Filtert het EMG signaal van de biceps { 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); //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 @@ -289,9 +289,9 @@ 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); //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 + arm_biquad_cascade_df1_init_f32(&highpass_biceps, 2, highpass_biceps_const, highpass_biceps_states); //Highpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters + arm_biquad_cascade_df1_init_f32(¬ch_biceps, 2, 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, 2, lowpass_biceps_const, lowpass_biceps_states); //Lowpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters 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 @@ -304,167 +304,748 @@ xb = EMG_bi.read(); //EMG meten van biceps - filter_biceps(); + filter_biceps(); //Voer acties uit om EMG signaal van de biceps te filteren - if(int(biceps_kalibratie.read()) == 1) { - lcd.locate(0,0); - lcd.printf(" 4 "); - pc.printf("4"); + if(int(biceps_kalibratie.read()) == 0) //Wanneer de timer nog geen een seconde heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 5 "); //Tekst op LCD scherm + pc.printf("4"); //Controle naar pc sturen + } + if(int(biceps_kalibratie.read()) == 1) //Wanneer de timer nog geen twee seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 4 "); //Tekst op LCD scherm + pc.printf("3"); //Controle naar pc sturen } - if(int(biceps_kalibratie.read()) == 2) { - lcd.locate(0,0); - lcd.printf(" 3 "); - pc.printf("3"); + if(int(biceps_kalibratie.read()) == 2) //Wanneer de timer nog geen drie seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 3 "); //Tekst op LCD scherm + pc.printf("2"); //Controle naar pc sturen } - if(int(biceps_kalibratie.read()) == 3) { - lcd.locate(0,0); - lcd.printf(" 2 "); - pc.printf("2"); + if(int(biceps_kalibratie.read()) == 3) //Wanneer de timer nog geen vier seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 2 "); //Tekst op LCD scherm + pc.printf("1"); //Controle naar pc sturen } - if(int(biceps_kalibratie.read()) == 4) { - lcd.locate(0,0); - lcd.printf(" 1 "); - pc.printf("1"); + if(int(biceps_kalibratie.read()) == 4) //Wanneer de timer nog geen vijf seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 1 "); //Tekst op LCD scherm + pc.printf("1"); //Controle naar pc sturen } } - if(xbf >= xbfmax) { - xbfmax = xbf; + if(xbf >= xbfmax) //Als de gefilterde EMG waarde groter is dan xbfmax + { + xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax } - xbt = xbfmax * 0.8; - pc.printf("threshold is %f\n\r", xbt); - state = EMG_KALIBRATIE_TRICEPS; - break; + + xbt = xbfmax * 0.8; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten + pc.printf("threshold is %f\n\r", xbt); //Thresholdwaarde naar pc sturen + state = EMG_KALIBRATIE_TRICEPS; //Gaat door naar kalibratie van de EMG van de triceps + break; //Stopt alle acties in deze case } - case EMG_KALIBRATIE_TRICEPS: { - pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); + case EMG_KALIBRATIE_TRICEPS: + { + pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); //Begin EMG_KALIBRATIE_TRICEPS naar pc sturen - lcd.locate(0,0); - lcd.printf(" SPAN IN 5 SEC. "); - lcd.locate(0,1); - lcd.printf(" 2 X TRICEPS AAN"); + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm + lcd.locate(0,1); //Zet tekst op eerste regel + lcd.printf(" 2 X TRICEPS AAN"); //Tekst op LCD scherm - 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); - arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1, lowpass_triceps_const, lowpass_triceps_states); + arm_biquad_cascade_df1_init_f32(&highpass_triceps, 2, highpass_triceps_const, highpass_triceps_states); //Highpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters + arm_biquad_cascade_df1_init_f32(¬ch_triceps, 2, notch_triceps_const, notch_triceps_states); //Notchfilter triceps met bijbehorende filtercoëfficiënten en states definiëren + arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 2, lowpass_triceps_const, lowpass_triceps_states); //Lowpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters - ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); - triceps_kalibratie.start(); + //ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Deze ticker loopt nog + triceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt - while(triceps_kalibratie.read() <= 5) { - + while(triceps_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 - xt = EMG_tri.read(); //EMG meten van biceps + xt = EMG_tri.read(); //EMG meten van ticeps - filter_triceps(); + filter_triceps(); //Voer acties uit om EMG signaal van de triceps te meten - if(triceps_kalibratie.read() == 1) { - lcd.locate(0,0); - lcd.printf(" 4 "); - pc.printf("4"); + if(int(triceps_kalibratie.read()) == 0) //Wanneer de timer nog geen twee seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 5 "); //Tekst op LCD scherm + pc.printf("4"); //Controle naar pc sturen + } + if(int(triceps_kalibratie.read()) == 1) //Wanneer de timer nog geen twee seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 4 "); //Tekst op LCD scherm + pc.printf("3"); //Controle naar pc sturen } - if(triceps_kalibratie.read() == 2) { - lcd.locate(0,0); - lcd.printf(" 3 "); - pc.printf("3"); + if(int(triceps_kalibratie.read()) == 2) //Wanneer de timer nog geen drie seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 3 "); //Tekst op LCD scherm + pc.printf("2"); //Controle naar pc sturen } - if(triceps_kalibratie.read() == 3) { - lcd.locate(0,0); - lcd.printf(" 2 "); - pc.printf("2"); + if(int(triceps_kalibratie.read()) == 3) //Wanneer de timer nog geen vier seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 2 "); //Tekst op LCD scherm + pc.printf("1"); //Controle naar pc sturen } - if(triceps_kalibratie.read() == 4) { - lcd.locate(0,0); - lcd.printf(" 1 "); - pc.printf("1"); + if(int(triceps_kalibratie.read()) == 4) //Wanneer de timer nog geen vijf seconden heeft gemeten + { + lcd.locate(0,0); //Zet de tekst op de eerste regel + lcd.printf(" 1 "); //Tekst op LCD scherm + pc.printf("1"); //Controle naar pc sturen } } - if(xtf >= xtfmax) { - xtfmax = xtf; + if(xtf >= xtfmax) //Als de gefilterde EMG waarde groter is dan xtfmax + { + xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax } - xtt = xtfmax * 0.8; - pc.printf("threshold is %f\n\r", xtt); - state = BEGIN_METEN; - break; + + xtt = xtfmax * 0.8; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten + pc.printf("threshold is %f\n\r", xtt); //Thresholdwaarde naar pc sturen + state = START; //Gaat door naar het slaan, kalibratie nu afgerond + break; //Stopt alle acties in deze case } - case BEGIN_METEN: { - lcd.locate(0,0); - lcd.printf(" START "); + case START: { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" START "); //Tekst op LCD scherm - pc.printf("START\n\r"); + pc.printf("START\n\r"); //Controle naar pc sturen - 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; + while(state == START) + { + 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 - xb = EMG_bi.read(); - filter_biceps(); - xt = EMG_tri.read(); - filter_triceps(); + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren - if(xb >= xbt) { - state = B; + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = B; //Ga door naar state B } - if(xt >= xtt) { - state = T; + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = T; //Ga door naar state T } } + break; //Stopt met de acties in deze case } - case B: { - lcd.locate(0,0); - lcd.printf(" B "); - pc.printf("B\n\r"); + case B: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" B "); //Tekst op LCD scherm + pc.printf("B\n\r"); //Controle naar pc sturen - 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; + 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; //Flag weer naar beneden, zodat deze straks weer omhoog kan - xb = EMG_bi.read(); - filter_biceps(); - xt = EMG_tri.read(); - filter_triceps(); + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren - if(xb >= xbt) { - state = BB; + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = BB; //Ga door naar state BB } - if(xt >= xtt) { - state = BT; + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = BT; //Ga door naar state BT } } + break; //Stop met alle acties in deze case } - case T: { - lcd.locate(0,0); - lcd.printf(" T "); - pc.printf("T\n\r"); + case T: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" T "); //Tekst op LCD scherm + pc.printf("T\n\r"); //Controle naar pc sturen - 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; + 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; //Flag weer naar beneden, zodat deze straks weer omhoog kan - xb = EMG_bi.read(); - filter_biceps(); - xt = EMG_tri.read(); - filter_triceps(); + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren - if(xb >= xbt) { - state = TB; + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = TB; //Ga door naar state TB } - if(xt >= xtt) { - state = TT; + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = TT; //Ga door naar state TT } } + break; //Stop met alle acties in deze case } case BB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BB "); //Tekst op LCD scherm + pc.printf("BB\n\r"); //Controle naar pc sturen + while(state == BB) + { + 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 + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = BBB; //Ga door naar state BBB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = BBT; //Ga door naar state BBT + } + } + break; //Stop met alle acties in deze case + } + + case BT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BT "); //Tekst op LCD scherm + pc.printf("BT\n\r"); //Controle naar pc sturen + + while(state == BT) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = BTB; //Ga door naar state BTB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = BTT; //Ga door naar state BTT + } + } + break; //Stop met alle acties in deze case + } + + case TB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TB "); //Tekst op LCD scherm + pc.printf("TB\n\r"); //Controle naar pc sturen + + while(state == TB) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = TBB; //Ga door naar state TBB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = TBT; //Ga door naar state TBT + } + } + break; //Stop met alle acties in deze case + } + + case TT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TT "); //Tekst op LCD scherm + pc.printf("TT\n\r"); //Controle naar pc sturen + + while(state == TT) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = TTB; //Ga door naar state TTB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = TTT; //Ga door naar state TTT + } + } + break; //Stop met alle acties in deze case + } + + case BBB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BBB "); //Tekst op LCD scherm + pc.printf("BBB\n\r"); //Controle naar pc sturen + + while(state == BBB) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = BBBB; //Ga door naar state BBBB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = BBBT; //Ga door naar state BBBT + } + } + break; //Stop met alle acties in deze case + } + + + case BBT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BBT "); //Tekst op LCD scherm + pc.printf("BBT\n\r"); //Controle naar pc sturen + + while(state == BBT) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = BBTB; //Ga door naar state BBTB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = BBTT; //Ga door naar state BBTT + } + } + break; //Stop met alle acties in deze case + } + + case BTB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BTB "); //Tekst op LCD scherm + pc.printf("BTB\n\r"); //Controle naar pc sturen + + while(state == BTB) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = BTBB; //Ga door naar state BTBB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = BTBT; //Ga door naar state BTBT + } + } + break; //Stop met alle acties in deze case + } + + case BTT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BTT "); //Tekst op LCD scherm + pc.printf("BTT\n\r"); //Controle naar pc sturen + + while(state == BTT) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = BTTB; //Ga door naar state BTTB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = BTTT; //Ga door naar state BTTT + } + } + break; //Stop met alle acties in deze case + } + + case TBB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TBB "); //Tekst op LCD scherm + pc.printf("TBB\n\r"); //Controle naar pc sturen + + while(state == TBB) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = TBBB; //Ga door naar state TBBB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = TBBT; //Ga door naar state TBBT + } + } + break; //Stop met alle acties in deze case + } + + case TBT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TBT "); //Tekst op LCD scherm + pc.printf("TBT\n\r"); //Controle naar pc sturen + + while(state == TBT) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = TBTB; //Ga door naar state TBTB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = TBTT; //Ga door naar state TBTT + } + } + break; //Stop met alle acties in deze case + } + + case TTB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BBB "); //Tekst op LCD scherm + pc.printf("BBB\n\r"); //Controle naar pc sturen + + while(state == BBB) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = TTBB; //Ga door naar state TTBB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = TTBT; //Ga door naar state TTBT + } + } + break; //Stop met alle acties in deze case + } + + case TTT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTT "); //Tekst op LCD scherm + pc.printf("TTT\n\r"); //Controle naar pc sturen + + while(state == TTT) + { + 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 + + xb = EMG_bi.read(); //EMG signaal biceps uitlezen + filter_biceps(); //EMG signaal biceps filteren + xt = EMG_tri.read(); //EMG signaal triceps uitlezen + filter_triceps(); //EMG signaal triceps filteren + + if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + { + state = TTTB; //Ga door naar state TTTB + } + if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + { + state = TTTT; //Ga door naar state TTTT + } + } + break; //Stop met alle acties in deze case + } + + case BBBB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BBBB "); //Tekst op LCD scherm + pc.printf("BBBB\n\r"); //Controle naar pc sturen + + while(state == BBBB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case BBBT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BBBT "); //Tekst op LCD scherm + pc.printf("BBBT\n\r"); //Controle naar pc sturen + + while(state == BBBT) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case BBTB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BBTB "); //Tekst op LCD scherm + pc.printf("BBTB\n\r"); //Controle naar pc sturen + + while(state == BBTB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case BBTT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BBTT "); //Tekst op LCD scherm + pc.printf("BBTT\n\r"); //Controle naar pc sturen + + while(state == BBTT) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case BTBB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BTBB "); //Tekst op LCD scherm + pc.printf("BTBB\n\r"); //Controle naar pc sturen + + while(state == BTBB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case BTBT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BTBT "); //Tekst op LCD scherm + pc.printf("BTBT\n\r"); //Controle naar pc sturen + + while(state == BTBT) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case BTTB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BTTB "); //Tekst op LCD scherm + pc.printf("BTTB\n\r"); //Controle naar pc sturen + + while(state == BTTB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case BTTT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" BTTT "); //Tekst op LCD scherm + pc.printf("BTTT\n\r"); //Controle naar pc sturen + + while(state == BTTT) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case TBBB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TBBB "); //Tekst op LCD scherm + pc.printf("TBBB\n\r"); //Controle naar pc sturen + + while(state == TBBB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case TBBT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TBBT "); //Tekst op LCD scherm + pc.printf("TBBT\n\r"); //Controle naar pc sturen + + while(state == TBBT) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case TBTB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TBBB "); //Tekst op LCD scherm + pc.printf("TBBB\n\r"); //Controle naar pc sturen + + while(state == TBBB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case TBTT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TBTT "); //Tekst op LCD scherm + pc.printf("TBTT\n\r"); //Controle naar pc sturen + + while(state == TBTT) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case TTBB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTBB "); //Tekst op LCD scherm + pc.printf("TTBB\n\r"); //Controle naar pc sturen + + while(state == TTBB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case TTBT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTBT "); //Tekst op LCD scherm + pc.printf("TTBT\n\r"); //Controle naar pc sturen + + while(state == TTBT) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case TTTB: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTTB "); //Tekst op LCD scherm + pc.printf("TTTB\n\r"); //Controle naar pc sturen + + while(state == TTTB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + + case TTTT: + { + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TTTT "); //Tekst op LCD scherm + pc.printf("TTBB\n\r"); //Controle naar pc sturen + + while(state == TTBB) + { + //Motoractie + } + break; //Stop met alle acties in deze case + } + 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