Janine Klein Rot
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
--- 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