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:
- 25:71e52c56be13
- Parent:
- 24:a1fdc830f96c
- Child:
- 26:438a498e5526
--- a/main.cpp Sun Nov 02 19:42:31 2014 +0000 +++ b/main.cpp Mon Nov 03 11:13:33 2014 +0000 @@ -6,13 +6,13 @@ //Constanten definiëren en waarde geven #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor +#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten #define KP_arm1 0.02 //Factor proprotionele regelaar arm 1 -#define KI_arm1 0 //Factor integraal regelaar arm 1 -#define KD_arm1 0 //Factor afgeleide regelaar arm 1 +#define KI_arm1 0 //Factor integraal regelaar arm 1 +#define KD_arm1 0 //Factor afgeleide regelaar arm 1 #define KP_arm2 0.01 //Factor proprotionele regelaar arm 2 #define KI_arm2 0 //Factor integraal regelaar arm 2 #define KD_arm2 0 //Factor afgeleide regelaar arm 2 -#define 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 @@ -54,16 +54,17 @@ 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 -Ticker ticker_EMG; //Ticker voor EMG meten +Ticker ticker_regelaar; //Ticker voor flag veranderen referentie +Ticker ticker_EMG; //Ticker voor flag EMG meten +Ticker ticker_motor_arm1_pid; //Ticker voor PID regelaar motor arm 1 +Ticker ticker_motor_arm2_pid; //Ticker voor PID regelaar motor arm 2 + Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps -Ticker ticker_motor_arm1_pid; -Ticker ticker_motor_arm2_pid; -Timer EMG; +Timer EMG; //Timer voor aantal seconden EMG meten //States definiëren -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, WACHT, THUISPOSITIE_MIDDEN}; //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, WACHT, THUISPOSITIE_MIDDEN, THUISPOSITIE_RECHTS}; //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,12 +80,11 @@ float afgeleide_arm2; //Afgeleide actie van regelaar arm 2 float xb; //Gemeten EMG waarde biceps float xt; //Gemeten EMG waarde triceps - float referentie_arm1 = 0; float referentie_arm2 = 0; - float t = 0; +//Gedefinieerde filters met constanten en gefilterde waarden 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 float highpass_biceps_states[8]; //Aantal states van het filter, het aantal y(n-x) en x(n-x) @@ -142,14 +142,14 @@ } } -void motor_arm1_pid() +void motor_arm1_pid() //PID regelaar van motor arm 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 - vorige_error_arm1 = error_arm1_kalibratie; - keep_in_range(&pwm_to_motor_arm1, -1, 1); + vorige_error_arm1 = error_arm1_kalibratie; //Vorige error is de volgende keer de huidige error van deze keer + keep_in_range(&pwm_to_motor_arm1, -1, 1); //Stelt 1 en -1 in als de maximale en minimale waarde die de pwm mag hebben if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1 dir_motor_arm1.write(0); @@ -160,14 +160,14 @@ pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM } -void motor_arm2_pid() +void motor_arm2_pid() //PID regelaar van motor arm 2 { 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 - vorige_error_arm2 = error_arm2_kalibratie; - keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) + vorige_error_arm2 = error_arm2_kalibratie; //Vorige error is de volgende keer de huidige error van deze keer + keep_in_range(&pwm_to_motor_arm2, -1, 1); //Stelt 1 en -1 in als de maximale en minimale waarde die de pwm mag hebben if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1 dir_motor_arm2.write(1); @@ -204,7 +204,7 @@ } -void EMG_meten() +void EMG_meten() //Meet de EMG van de biceps en de triceps { xb = EMG_bi.read(); //EMG signaal biceps uitlezen filter_biceps(); //EMG signaal biceps filteren @@ -215,8 +215,8 @@ int main() //Main script { - ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR); - ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); + ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR); //Iedere SAMPLETIME_REGELAAR wordt door de PID regelaar bekeken of er een andere referentie is en stuurt indien nodig een pwm naar de motor + ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); //Iederen SAMPLETIME_REGELAAR wordt door de PID regelaar bekeken of er een andere referentie is en stuurt indien nodig een pwm naar de motor 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 @@ -232,7 +232,7 @@ wait(2); //Twee seconden wachten lcd.cls(); //Maak LCD scherm leeg pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten - state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel + state = KALIBRATIE_ARM1; //Door naar de volgende state break; //Stopt acties in deze case } @@ -246,17 +246,16 @@ 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 - referentie_arm1 = referentie_arm1 + 180.0/200.0; - - pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen - pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen - pc.printf("referentie1 %f\n\r", referentie_arm1); + referentie_arm1 = referentie_arm1 + 180.0/200.0; //Referentie loopt in één seconde op tot het gewenste aantal pulsen - if (referentie_arm1 >= 180) { - referentie_arm1 = 180; - state = KALIBRATIE_ARM2; + pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen + pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen + pc.printf("referentie1 %f\n\r", referentie_arm1); //Huidige referentie naar pc sturen + + if (puls_motor_arm1.getPosition() >= 180) { //Als het gewenste aantal pulsen behaald is + referentie_arm1 = 180; //Blijft de referentie op dat aantal pulsen staan + state = KALIBRATIE_ARM2; //Door naar de volgende state } - } wait(1); //Een seconde wachten break; //Stopt acties in deze case @@ -272,20 +271,18 @@ 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 - referentie_arm2 = referentie_arm2 + 123.0/200.0; - - pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen - pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen - pc.printf("referentie2 %f\n\r", referentie_arm2); + referentie_arm2 = referentie_arm2 + 123.0/200.0; //Referentie loopt in één seconde op tot het gewenste aantal pulsen - if(referentie_arm2 >= 123) { - referentie_arm2 = 123; - state = EMG_KALIBRATIE_BICEPS; + pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen + pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen + pc.printf("referentie2 %f\n\r", referentie_arm2); //Huidige referentie naar pc sturen + + if(puls_motor_arm2.getPosition() >= 123) { //Als het gewenste aantal pulsen behaald is + referentie_arm2 = 123; //Blijft de referentie op dat aantal pulsen staan + state = EMG_KALIBRATIE_BICEPS; //Door naar de volgende state } - } wait(1); //Een seconde wachten - //ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer break; //Stopt acties in deze case } @@ -296,8 +293,8 @@ lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm lcd.locate(0,1); //Zet tekst op tweede regel lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm - wait(1); - lcd.cls(); + wait(1); //Een seconden wachten + lcd.cls(); //LCD scherm leegmaken 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 @@ -346,28 +343,27 @@ } } - xbt = xbfmax * 0.95; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten - pc.printf("maximale biceps %f", xbfmax); + xbt = xbfmax * 0.90; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten + pc.printf("maximale biceps %f", xbfmax); //Maximale gefilterde EMG waarde naar pc sturen 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: { //Kalibratie triceps voor bepalen threshold 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 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 - wait(1); - lcd.cls(); + wait(1); //Een seconde wachten + lcd.cls(); //LCD scherm leegmaken 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 @@ -411,13 +407,13 @@ } xtt = xtfmax * 0.90; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten - pc.printf("maximale triceps %f", xtfmax); + pc.printf("maximale triceps %f", xtfmax); //Maximale gefilterde EMG waarde naar pc sturen 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 START: { + case START: { //Eerste keuze maken voor doel lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" START "); //Tekst op LCD scherm @@ -432,37 +428,37 @@ xt = EMG_tri.read(); //EMG signaal triceps uitlezen filter_triceps(); //EMG signaal triceps filteren - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = B; //Ga door naar state B } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { //Tweede keuze maken voor doel 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 - EMG.start(); + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BB; //Ga door naar state BB } if(xtf>= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold @@ -472,80 +468,80 @@ break; //Stop met alle acties in deze case } - case T: { + case T: { //Tweede keuze maken voor doel 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 - EMG.start(); + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TB; //Ga door naar state TB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case BB: { //Derde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BBB; //Ga door naar state BBB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case BT: { //Derde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } while(state == BT) { @@ -554,103 +550,103 @@ EMG_meten(); - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BTB; //Ga door naar state BTB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case TB: { //Derde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TBB; //Ga door naar state TBB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case TT: { //Derde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TTB; //Ga door naar state TTB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case BBB: { //Vierde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BBBB; //Ga door naar state BBBB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold state = BBBT; //Ga door naar state BBBT } } @@ -658,150 +654,150 @@ } - case BBT: { + case BBT: { //Vierde keuze maken in doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BBTB; //Ga door naar state BBTB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case BTB: { //Vierde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BTBB; //Ga door naar state BTBB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case BTT: { //Vierde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = BTTB; //Ga door naar state BTTB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case TBB: { //Vierde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps - if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold + if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TBBB; //Ga door naar state TBBB } - if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold + if(xtf >= 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: { + case TBT: { //Vierde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TBTB; //Ga door naar state TBTB @@ -813,26 +809,26 @@ break; //Stop met alle acties in deze case } - case TTB: { + case TTB: { //Vierde keuze maken voor doel lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" TTB "); //Tekst op LCD scherm pc.printf("TTB\n\r"); //Controle naar pc sturen - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } while(state == TTB) { 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TTBB; //Ga door naar state TTBB @@ -844,26 +840,26 @@ break; //Stop met alle acties in deze case } - case TTT: { + case TTT: { //Vierde keuze maken voor doel 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 - EMG.reset(); - EMG.start(); + EMG.reset(); //Timer resetten + EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 3) { + while(EMG.read() <= 2) { //Timer nog geen drie seconden 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps } 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 - EMG_meten(); + EMG_meten(); //EMG meten van biceps en triceps if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold state = TTTB; //Ga door naar state TTTB @@ -875,140 +871,247 @@ break; //Stop met alle acties in deze case } - case BBBB: { + case BBBB: { //Motoraansturing voor doel rechtsonder 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 + 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 + + while(puls_motor_arm1.getPosition() > -84) { + referentie_arm1 = referentie_arm1 - 264.0/200.0; + pc.printf("referentie arm 1 %f ", referentie_arm1); + pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); + pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); + } + pc.printf("arm 1 op positie"); + + while(puls_motor_arm2.getPosition() < 211) { + referentie_arm2 = referentie_arm2 + 88.0/200.0; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); + } + pc.printf("arm2 op positie"); + + while(puls_motor_arm2.getPosition() > 36) { + + referentie_arm2 = -0.5 * 100000 * t * t; + t = t + 0.005; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + } + + if(puls_motor_arm2.getPosition() <= 36) { + wait(1); + while(puls_motor_arm2.getPosition() < 211) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + + if(puls_motor_arm2.getPosition() >= 211) { + pc.printf("staat stil\n\r"); + state = THUISPOSITIE_RECHTS; + } + } } break; //Stop met alle acties in deze case } - case BBBT: { + case BBBT: { //Geen motoraansturing 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 + lcd.printf(" GEEN DOEL "); + wait(1); + state = WACHT; } break; //Stop met alle acties in deze case } - case BBTB: { + case BBTB: { //Motoraansturing voor doel rechtsmidden 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 + 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 + + while(puls_motor_arm1.getPosition() > -84) { + referentie_arm1 = referentie_arm1 - 264.0/200.0; + pc.printf("referentie arm 1 %f ", referentie_arm1); + pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); + pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); + } + pc.printf("arm 1 op positie"); + + while(puls_motor_arm2.getPosition() < 211) { + referentie_arm2 = referentie_arm2 + 88.0/200.0; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); + } + pc.printf("arm2 op positie"); + + + + while(puls_motor_arm2.getPosition() > 36) { + + referentie_arm2 = -0.5 * 150000 * t * t; + t = t + 0.005; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + } + + if(puls_motor_arm2.getPosition() <= 36) { + wait(1); + while(puls_motor_arm2.getPosition() < 211) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + + if(puls_motor_arm2.getPosition() >= 211) { + pc.printf("staat stil\n\r"); + state = THUISPOSITIE_RECHTS; + } + } } break; //Stop met alle acties in deze case } - case BBTT: { + case BBTT: { //Motoraansturing voor doel rechtsboven 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 + 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 + + while(puls_motor_arm1.getPosition() > -84) { + referentie_arm1 = referentie_arm1 - 264.0/200.0; + pc.printf("referentie arm 1 %f ", referentie_arm1); + pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); + pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); + } + pc.printf("arm 1 op positie"); + + while(puls_motor_arm2.getPosition() < 211) { + referentie_arm2 = referentie_arm2 + 88.0/200.0; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); + } + pc.printf("arm2 op positie"); + + while(puls_motor_arm2.getPosition() > 36) { + + referentie_arm2 = -0.5 * 200000 * t * t; + t = t + 0.005; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + } + + if(puls_motor_arm2.getPosition() <= 36) { + wait(1); + while(puls_motor_arm2.getPosition() < 211) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + + if(puls_motor_arm2.getPosition() >= 211) { + pc.printf("staat stil\n\r"); + state = THUISPOSITIE_RECHTS; + } + } } break; //Stop met alle acties in deze case } - case BTBB: { + case BTBB: { //Geen motoraansturing 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 + lcd.printf(" GEEN DOEL "); + wait(1); + state = WACHT; } break; //Stop met alle acties in deze case } - case BTBT: { + case BTBT: { //Geen motoraansturing 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 + lcd.printf(" GEEN DOEL "); + wait(1); + state = WACHT; } break; //Stop met alle acties in deze case } - case BTTB: { + case BTTB: { //Geen motoraansturing 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 + lcd.printf(" GEEN DOEL "); + wait(1); + state = WACHT; } break; //Stop met alle acties in deze case } - case BTTT: { + case BTTT: { //Geen motoraansturing 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 + lcd.printf(" GEEN DOEL "); + wait(1); + state = WACHT; } break; //Stop met alle acties in deze case } - case TBBB: { + case TBBB: { //Motoraansturing voor doel middenonder 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(" TBTB "); //Tekst op LCD scherm - pc.printf("TBTB\n\r"); //Controle naar pc sturen - - while(state == TBTB) { - //Motoractie - } - break; //Stop met alle acties in deze case - } - - case TBTT: { //Motoraansturing voor richten op doel middenboven - 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 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 - while(puls_motor_arm1.getPosition() > 48) { referentie_arm1 = referentie_arm1 - 132.0/200.0; pc.printf("referentie arm 1 %f ", referentie_arm1); @@ -1025,57 +1128,169 @@ } pc.printf("arm2 op positie"); + while(puls_motor_arm2.getPosition() > -8) { - if(referentie_arm2 >= 167) { - while(puls_motor_arm2.getPosition() > -8) { + referentie_arm2 = -0.5 * 100000 * t * t; + t = t + 0.005; - + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + } - referentie_arm2 = -0.5 * 200000 * t * t; - t = t + 0.005; + if(puls_motor_arm2.getPosition() <= -8) { + wait(1); + while(puls_motor_arm2.getPosition() < 167) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; pc.printf("referentie arm 2 %f ", referentie_arm2); pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); - pc.printf("t is %f\n\r", t); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); } - - if(puls_motor_arm2.getPosition() <= -8) { - wait(1); - while(puls_motor_arm2.getPosition() < 167) { - referentie_arm2 = referentie_arm2 + 167.0/200.0; - - - pc.printf("referentie arm 2 %f ", referentie_arm2); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); - } - - if(puls_motor_arm2.getPosition() >= 167) { - pc.printf("staat stil\n\r"); - state = WACHT; - } + if(puls_motor_arm2.getPosition() >= 167) { + pc.printf("staat stil\n\r"); + state = THUISPOSITIE_MIDDEN; } } } break; //Stop met alle acties in deze case } - case TTBB: { //Motoraansturing voor richten op doel linksonder + case TBBT: { //Geen motoraansturing + 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) { + lcd.printf(" GEEN DOEL "); + wait(1); + state = WACHT; + } + break; //Stop met alle acties in deze case + } + + case TBTB: { //Motoraansturing voor doel midden + lcd.locate(0,0); //Zet tekst op eerste regel + lcd.printf(" TBTB "); //Tekst op LCD scherm + pc.printf("TBTB\n\r"); //Controle naar pc sturen + + while(state == TBTB) { + 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 + + while(puls_motor_arm1.getPosition() > 48) { + referentie_arm1 = referentie_arm1 - 132.0/200.0; + pc.printf("referentie arm 1 %f ", referentie_arm1); + pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); + pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); + } + pc.printf("arm 1 op positie"); + + while(puls_motor_arm2.getPosition() < 167) { + referentie_arm2 = referentie_arm2 + 44.0/200.0; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); + } + pc.printf("arm2 op positie"); + + while(puls_motor_arm2.getPosition() > -8) { + + referentie_arm2 = -0.5 * 150000 * t * t; + t = t + 0.005; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + } + + if(puls_motor_arm2.getPosition() <= -8) { + wait(1); + while(puls_motor_arm2.getPosition() < 167) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + + if(puls_motor_arm2.getPosition() >= 167) { + pc.printf("staat stil\n\r"); + state = THUISPOSITIE_MIDDEN; + } + } + } + break; //Stop met alle acties in deze case + } + + case TBTT: { //Motoraansturing voor doel middenboven + 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(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 + + while(puls_motor_arm1.getPosition() > 48) { + referentie_arm1 = referentie_arm1 - 132.0/200.0; + pc.printf("referentie arm 1 %f ", referentie_arm1); + pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); + pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); + } + pc.printf("arm 1 op positie"); + + while(puls_motor_arm2.getPosition() < 167) { + referentie_arm2 = referentie_arm2 + 44.0/200.0; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); + } + pc.printf("arm2 op positie"); + + while(puls_motor_arm2.getPosition() > -8) { + + referentie_arm2 = -0.5 * 200000 * t * t; + t = t + 0.005; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); + pc.printf("t is %f\n\r", t); + } + + if(puls_motor_arm2.getPosition() <= -8) { + wait(1); + while(puls_motor_arm2.getPosition() < 167) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; + + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + } + + if(puls_motor_arm2.getPosition() >= 167) { + pc.printf("staat stil\n\r"); + state = THUISPOSITIE_MIDDEN; + } + } + } + break; //Stop met alle acties in deze case + } + + case TTBB: { //Motoraansturing voor doel linksonder 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 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 - - referentie_arm2 = 0.5 * 100000 * t * t; + referentie_arm2 = -0.5 * 100000 * t * t; t = t + 0.005; pc.printf("referentie arm 2 %f ", referentie_arm2); @@ -1083,35 +1298,31 @@ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); pc.printf("t is %f\n\r", t); - if(puls_motor_arm2.getPosition() >= 175) { + if(puls_motor_arm2.getPosition() <= -175) { wait(1); - while(puls_motor_arm2.getPosition() > 0) { - referentie_arm2 = referentie_arm2 - 175.0/200.0; - + while(puls_motor_arm2.getPosition() < 0) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; pc.printf("referentie arm 2 %f ", referentie_arm2); pc.printf("get position %d ", puls_motor_arm2.getPosition()); pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); } - - if(puls_motor_arm2.getPosition() <= 0) { + if(puls_motor_arm2.getPosition() >= 0) { pc.printf("staat stil\n\r"); state = WACHT; } - } } break; //Stop met alle acties in deze case } - case TTBT: { //Motoraansturing voor terug naar kalibratiepositie + case TTBT: { //Geen motoraansturing 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 lcd.printf(" GEEN DOEL "); wait(1); state = WACHT; @@ -1119,21 +1330,17 @@ break; //Stop met alle acties in deze case } - case TTTB: { //Motoraansturing voor richten op doel linksmidden + case TTTB: { //Motoraansturing voor doel linksmidden 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 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 - - referentie_arm2 = 0.5 * 150000 * t * t; + referentie_arm2 = -0.5 * 150000 * t * t; t = t + 0.005; pc.printf("referentie arm 2 %f ", referentie_arm2); @@ -1141,31 +1348,27 @@ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); pc.printf("t is %f\n\r", t); - if(puls_motor_arm2.getPosition() >= 175) { + if(puls_motor_arm2.getPosition() <= -175) { wait(1); - while(puls_motor_arm2.getPosition() > 0) { - referentie_arm2 = referentie_arm2 - 175.0/200.0; - + while(puls_motor_arm2.getPosition() < 0) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; pc.printf("referentie arm 2 %f ", referentie_arm2); pc.printf("get position %d ", puls_motor_arm2.getPosition()); pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); } - - if(puls_motor_arm2.getPosition() <= 0) { + if(puls_motor_arm2.getPosition() >= 0) { pc.printf("staat stil\n\r"); state = WACHT; } - } } break; //Stop met alle acties in deze case } - - case TTTT: { //Motoraansturing voor richten op doel linksboven + case TTTT: { //Motoraansturing voor doel linksboven 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 @@ -1174,10 +1377,7 @@ 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 - - referentie_arm2 = 0.5 * 200000 * t * t; + referentie_arm2 = -0.5 * 200000 * t * t; t = t + 0.005; pc.printf("referentie arm 2 %f ", referentie_arm2); @@ -1185,10 +1385,10 @@ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); pc.printf("t is %f\n\r", t); - if(puls_motor_arm2.getPosition() >= 175) { + if(puls_motor_arm2.getPosition() <= -175) { wait(1); - while(puls_motor_arm2.getPosition() > 0) { - referentie_arm2 = referentie_arm2 - 175.0/200.0; + while(puls_motor_arm2.getPosition() < 0) { + referentie_arm2 = referentie_arm2 + 175.0/200.0; pc.printf("referentie arm 2 %f ", referentie_arm2); @@ -1196,37 +1396,69 @@ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); } - - if(puls_motor_arm2.getPosition() <= 0) { + if(puls_motor_arm2.getPosition() >= 0) { pc.printf("staat stil\n\r"); state = WACHT; } - } } } case THUISPOSITIE_MIDDEN: { while(puls_motor_arm2.getPosition() > 123) { + 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 + referentie_arm2 = referentie_arm2 - 44.0/200.0; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); } while(puls_motor_arm1.getPosition() < 180) { + 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 + referentie_arm1 = referentie_arm1 + 132.0/200.0; + pc.printf("referentie arm 1 %f ", referentie_arm1); + pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); + pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); + } state = WACHT; break; } + case THUISPOSITIE_RECHTS: { + pc.printf("thuispositie rechts"); + while(puls_motor_arm2.getPosition() > 123) { + 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 + + referentie_arm2 = referentie_arm2 - 88.0/200.0; + pc.printf("referentie arm 2 %f ", referentie_arm2); + pc.printf("get position %d ", puls_motor_arm2.getPosition()); + pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + + } + + while(puls_motor_arm1.getPosition() < 180) { + 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 + + referentie_arm1 = referentie_arm1 + 264.0/200.0; + pc.printf("referentie arm 1 %f ", referentie_arm1); + pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); + pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); + } + } + case WACHT: { lcd.printf(" WACHT "); wait(5); state = START; - - - 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