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:
- 19:38c9d177b6ee
- Parent:
- 18:d3a7f8b4cb22
- Child:
- 20:1cb0cf0d49ac
--- a/main.cpp Thu Oct 30 11:35:04 2014 +0000 +++ b/main.cpp Thu Oct 30 16:12:46 2014 +0000 @@ -6,15 +6,13 @@ //Constanten definiëren en waarde geven #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor -#define KP_arm1 //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 4 //Factor proprotionele regelaar arm 2 -#define KI_arm2 0.5 //Factor integraal regelaar arm 2 -#define KD_arm2 0.5 //Factor afgeleide regelaar arm 2 -#define SAMPLETIME_EMG 0 //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 +#define KP_arm1 0.5 //Factor proprotionele regelaar arm 1 +#define KI_arm1 0.1 //Factor integraal regelaar arm 1 +#define KD_arm1 0.01 //Factor afgeleide regelaar arm 1 +#define KP_arm2 0.3 //Factor proprotionele regelaar arm 2 +#define KI_arm2 0.1 //Factor integraal regelaar arm 2 +#define KD_arm2 0.1 //Factor afgeleide regelaar arm 2 +#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten //Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk //High Pass filter @@ -79,7 +77,8 @@ float xb; //Gemeten EMG waarde biceps float xt; //Gemeten EMG waarde triceps -float puls_arm1_home = 0; +float referentie_arm1 = 0; +float referentie_arm2 = 0; 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, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter @@ -128,72 +127,64 @@ 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() //Brengt arm 1 naar de beginpositie { - puls_arm1_home = puls_arm1_home + 180/200; - error_arm1_kalibratie = (puls_arm1_home - 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) + referentie_arm1 = referentie_arm1 + 180.0/200.0; + + if (referentie_arm1 >= 180) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) + referentie_arm1 = 180; - if (pwm_to_motor_arm1 > 0) //Als PWM is positief, dan richting 1 - { + } + error_arm1_kalibratie = (referentie_arm1 - 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); + + 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_to_motor_arm1=pwm_to_motor_arm1/20; 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()); //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 (error_arm1_kalibratie == 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 - } + 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 + } 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 + referentie_arm2 = referentie_arm2 + 35/200; + error_arm2_kalibratie = (referentie_arm2 - 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()); //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) - { + 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 } @@ -202,13 +193,13 @@ 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 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 } @@ -228,16 +219,14 @@ int main() //Main script { - while(1) //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan - { + 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 - { - pc.printf("RUST\n\r"); //Begin RUST naar pc sturen + 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 @@ -249,15 +238,13 @@ break; //Stopt acties in deze case } - case KALIBRATIE_ARM1: //Arm 1 naar thuispositie - { + 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(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 @@ -267,15 +254,13 @@ break; //Stopt acties in deze case } - case KALIBRATIE_ARM2: //Arm 2 naar thuispositie - { - pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen + case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie + pc.printf("KALIBRATIE_ARM2\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(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 @@ -286,10 +271,9 @@ break; //Stopt acties in deze case } - case EMG_KALIBRATIE_BICEPS: //Kalibratie biceps voor bepalen threshold - { + 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 @@ -302,9 +286,8 @@ 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) //Zolang de timer nog geen 5 seconden heeft gemeten - { - + 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 @@ -312,51 +295,44 @@ filter_biceps(); //Voer acties uit om EMG signaal van de biceps te filteren - if(int(biceps_kalibratie.read()) == 0) //Wanneer de timer nog geen een seconde heeft gemeten - { + 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 - { + 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) //Wanneer de timer nog geen drie seconden heeft gemeten - { + 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) //Wanneer de timer nog geen vier seconden heeft gemeten - { + 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) //Wanneer de timer nog geen vijf seconden heeft gemeten - { + 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) //Als de gefilterde EMG waarde groter is dan xbfmax - { + 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; //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: - { + case EMG_KALIBRATIE_TRICEPS: { pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); //Begin EMG_KALIBRATIE_TRICEPS naar pc sturen lcd.locate(0,0); //Zet de tekst op de eerste regel @@ -364,15 +340,14 @@ 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, 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(&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); //Deze ticker loopt nog triceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt - while(triceps_kalibratie.read() <= 5) //Zolang de timer nog geen 5 seconden heeft gemeten - { + 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 @@ -380,43 +355,37 @@ filter_triceps(); //Voer acties uit om EMG signaal van de triceps te meten - if(int(triceps_kalibratie.read()) == 0) //Wanneer de timer nog geen twee seconden heeft gemeten - { + 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 - { + 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(int(triceps_kalibratie.read()) == 2) //Wanneer de timer nog geen drie seconden heeft gemeten - { + 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(int(triceps_kalibratie.read()) == 3) //Wanneer de timer nog geen vier seconden heeft gemeten - { + 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(int(triceps_kalibratie.read()) == 4) //Wanneer de timer nog geen vijf seconden heeft gemeten - { + 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) //Als de gefilterde EMG waarde groter is dan xtfmax - { + 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; //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 @@ -426,11 +395,10 @@ case START: { lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" START "); //Tekst op LCD scherm - + pc.printf("START\n\r"); //Controle naar pc sturen - - while(state == START) - { + + 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 @@ -439,26 +407,46 @@ 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 - { + 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) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold - { + 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: - { + + case B: { lcd.locate(0,0); //Zet tekst op eerste regel - lcd.printf(" B "); //Tekst op LCD scherm + lcd.printf(" B "); //Tekst op LCD scherm pc.printf("B\n\r"); //Controle naar pc sturen - while(state == B) - { + 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(); //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 = BB; //Ga door naar state BB + } + 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); //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; //Flag weer naar beneden, zodat deze straks weer omhoog kan @@ -467,26 +455,22 @@ 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 = BB; //Ga door naar state BB + 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) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold - { - state = BT; //Ga door naar state BT + 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 T: - { + case BB: { 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 + lcd.printf(" BB "); //Tekst op LCD scherm + pc.printf("BB\n\r"); //Controle naar pc sturen - while(state == T) - { + 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 @@ -495,26 +479,22 @@ 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 = TB; //Ga door naar state TB + 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 = TT; //Ga door naar state TT + 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 BB: - { + + case BT: { 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 + lcd.printf(" BT "); //Tekst op LCD scherm + pc.printf("BT\n\r"); //Controle naar pc sturen - while(state == BB) - { + 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 @@ -523,26 +503,22 @@ 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(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 = BBT; //Ga door naar state BBT + 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 + break; //Stop met alle acties in deze case } - - case BT: - { + + case TB: { 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 + lcd.printf(" TB "); //Tekst op LCD scherm + pc.printf("TB\n\r"); //Controle naar pc sturen - while(state == BT) - { + 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 @@ -551,26 +527,22 @@ 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(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 = BTT; //Ga door naar state BTT + 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 - } + break; //Stop met alle acties in deze case + } - case TB: - { + case TT: { 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 + lcd.printf(" TT "); //Tekst op LCD scherm + pc.printf("TT\n\r"); //Controle naar pc sturen - while(state == TB) - { + 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 @@ -579,26 +551,22 @@ 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(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 = TBT; //Ga door naar state TBT + 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 + break; //Stop met alle acties in deze case } - - case TT: - { + + case BBB: { 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 + lcd.printf(" BBB "); //Tekst op LCD scherm + pc.printf("BBB\n\r"); //Controle naar pc sturen - while(state == TT) - { + 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 @@ -607,26 +575,23 @@ 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(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 = TTT; //Ga door naar state TTT + 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 + break; //Stop met alle acties in deze case } - case BBB: - { + + case BBT: { 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 + lcd.printf(" BBT "); //Tekst op LCD scherm + pc.printf("BBT\n\r"); //Controle naar pc sturen - while(state == BBB) - { + 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 @@ -635,27 +600,22 @@ 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(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 = BBBT; //Ga door naar state BBBT + 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 + break; //Stop met alle acties in deze case } - - - case BBT: - { + + case BTB: { 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 + lcd.printf(" BTB "); //Tekst op LCD scherm + pc.printf("BTB\n\r"); //Controle naar pc sturen - while(state == BBT) - { + 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 @@ -664,26 +624,22 @@ 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(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 = BBTT; //Ga door naar state BBTT + 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 + break; //Stop met alle acties in deze case } - - case BTB: - { + + case BTT: { 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 + lcd.printf(" BTT "); //Tekst op LCD scherm + pc.printf("BTT\n\r"); //Controle naar pc sturen - while(state == BTB) - { + 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 @@ -692,26 +648,22 @@ 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(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 = BTBT; //Ga door naar state BTBT + 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 BTT: - { + break; //Stop met alle acties in deze case + } + + case TBB: { 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 + lcd.printf(" TBB "); //Tekst op LCD scherm + pc.printf("TBB\n\r"); //Controle naar pc sturen - while(state == BTT) - { + 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 @@ -720,26 +672,22 @@ 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(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 = BTTT; //Ga door naar state BTTT + 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 + break; //Stop met alle acties in deze case } - - case TBB: - { + + case TBT: { 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 + lcd.printf(" TBT "); //Tekst op LCD scherm + pc.printf("TBT\n\r"); //Controle naar pc sturen - while(state == TBB) - { + 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 @@ -748,26 +696,22 @@ 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(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 = TBBT; //Ga door naar state TBBT + 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 + break; //Stop met alle acties in deze case } - - case TBT: - { + + case TTB: { 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 + lcd.printf(" BBB "); //Tekst op LCD scherm + pc.printf("BBB\n\r"); //Controle naar pc sturen - while(state == TBT) - { + 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 @@ -776,54 +720,22 @@ 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 - { + 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 - { + 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: - { + 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(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 @@ -832,232 +744,238 @@ 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 - { + 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 - { + 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 + break; //Stop met alle acties in deze case } - - case BBBB: - { + + 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) - { + while(state == BBBB) { //Motoractie - } - break; //Stop met alle acties in deze case - } - - case BBBT: - { + } + 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) - { + while(state == BBBT) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case BBTB: - { + + 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) - { + while(state == BBTB) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case BBTT: - { + + 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) - { + while(state == BBTT) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case BTBB: - { + + 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) - { + while(state == BTBB) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case BTBT: - { + + 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) - { + while(state == BTBT) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case BTTB: - { + + 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) - { + while(state == BTTB) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case BTTT: - { + + 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) - { + while(state == BTTT) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case TBBB: - { + + 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) - { + while(state == TBBB) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case TBBT: - { + + 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) - { + while(state == TBBT) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case TBTB: - { + + 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) - { + while(state == TBBB) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case TBTT: - { + + 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) - { + while(state == TBTT) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case TTBB: - { + + 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) - { + while(state == TTBB) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case TTBT: - { + + 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) - { + while(state == TTBT) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case TTTB: - { + + 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) - { + while(state == TTTB) { //Motoractie - } - break; //Stop met alle acties in deze case + } + break; //Stop met alle acties in deze case } - - case TTTT: - { + + 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 + while(state == TTTT) { + 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 + + //Positie arm 1 is goed + //Snelheid arm 2 + + float referentie_speed_TTTT; + float error_speed_TTTT; + float integraal_speed_TTTT = 0; + float afgeleide_speed_TTTT; + float vorige_error_speed_TTTT = 0; + float pwm_to_motor_speed_TTTT; +#define KPs 0.001 +#define KIs 0 +#define KDs 0 + + referentie_speed_TTTT = referentie_speed_TTTT + 1902/40; + error_speed_TTTT = (referentie_speed_TTTT - puls_motor_arm2.getSpeed()); + integraal_speed_TTTT = integraal_speed_TTTT + error_speed_TTTT*SAMPLETIME_REGELAAR; + afgeleide_speed_TTTT = (error_speed_TTTT - vorige_error_speed_TTTT)/SAMPLETIME_REGELAAR; + pwm_to_motor_speed_TTTT = error_speed_TTTT*KPs + integraal_speed_TTTT*KIs + afgeleide_speed_TTTT*KDs; + keep_in_range(&pwm_to_motor_speed_TTTT, -1, 1); + + if(pwm_to_motor_speed_TTTT > 0) { + dir_motor_arm2.write(1); + } else { + dir_motor_arm2.write(0); + } + + pwm_motor_arm1.write(0); + + if(puls_motor_arm2.getPosition() >= 97) { + pwm_motor_arm1.write(-fabs(pwm_to_motor_speed_TTTT)); + } + + if(referentie_speed_TTTT == 1902) { + referentie_speed_TTTT = 1902; + } + + referentie_arm1 = referentie_arm1 + 180/200; + + 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 + } + + pc.printf("state: %u\n\r",state); } - - 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); + //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) } - } -} \ No newline at end of file + } \ No newline at end of file