2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp
- Committer:
- JKleinRot
- Date:
- 2014-11-04
- Revision:
- 30:f79cf70e2917
- Parent:
- 29:f95f0cc84349
- Child:
- 31:36fe36657f8d
File content as of revision 30:f79cf70e2917:
#include "mbed.h" //Mbed bibliotheek inladen, standaard functies #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder #include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm #include "arm_math.h" //Filter bibliotheek inladen, makkelijk de filters maken, minder grote lappen tekst //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.025 //Factor proprotionele regelaar arm 1 #define KI_arm1 0 //Factor integraal regelaar arm 1 #define KD_arm1 0 //Factor afgeleide regelaar arm 1 #define KP_arm2 0.01 //Factor proprotionele regelaar arm 2 #define KI_arm2 0 //Factor integraal regelaar arm 2 #define KD_arm2 0.0008 //Factor afgeleide regelaar arm 2 //Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk //High Pass filter #define A1 1 #define A2 -1.561018075800718 #define A3 0.641351538057563 #define B1 0.800592403464570 #define B2 -1.601184806929141 #define B3 0.800592403464570 //Notch filter #define C1 1 #define C2 -1.18733334554802E-16 #define C3 0.939062505817492 #define D1 0.969531252908746 #define D2 -1.18733334554802E-16 #define D3 0.969531252908746 //Low pass filter #define E1 1 #define E2 -1.77863177782459 #define E3 0.800802646665708 #define F1 0.00554271721028068 #define F2 0.0110854344205614 #define F3 0.00554271721028068 //Pinverdeling en naamgeving variabelen TextLCD lcd(PTD1, PTA12, PTB2, PTB3, PTC2, PTD3, TextLCD::LCD16x2); //LCD scherm MODSERIAL pc(USBTX, USBRX); //PC PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1 Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1, (geel, wit) PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2 Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit) AnalogIn EMG_bi(PTB1); //Uitlezen EMG signaal biceps AnalogIn EMG_tri(PTB0); //Uitlezen EMG signaal triceps //Blauw op 3,3 V en groen op GND, voeding beide encoders 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 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, 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 float pwm_to_motor_arm1; //PWM output naar motor arm 1 float pwm_to_motor_arm2; //PWM output naar motor arm 2 float error_arm1_kalibratie; //Error in pulsen arm 1 float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1 float integraal_arm1 = 0; //Integraal actie van regelaar arm 1 float afgeleide_arm1; //Afgeleide actie van regelaar arm 1 float error_arm2_kalibratie; //Error in pulsen arm 2 float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2 float integraal_arm2 = 0; //Integraal actie van regelaar arm 2 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2 float xb; //Gemeten EMG waarde biceps float xt; //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) arm_biquad_casd_df1_inst_f32 notch_biceps; //Notch_biceps IIR filter in direct form 1 float notch_biceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter float notch_biceps_states[8]; //Aantal states van het filter arm_biquad_casd_df1_inst_f32 lowpass_biceps; //Lowpass_biceps IIR filter in direct form 1 float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter float lowpass_biceps_states[8]; //Aantal states van het filter float xbf; //Gefilterde EMG waarde biceps float xbfmax = 0; //Maximale gefilterde EMG waarde biceps arm_biquad_casd_df1_inst_f32 highpass_triceps; //Highpass_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter float highpass_triceps_states[8]; //Aantal states van het filter arm_biquad_casd_df1_inst_f32 notch_triceps; //Notch_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float float notch_triceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter float notch_triceps_states[8]; //Aantal states van het filter arm_biquad_casd_df1_inst_f32 lowpass_triceps; //Lowpass_biceps IIR filter in direct form 1, waarde wordt opgeslagen in een float float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter float lowpass_triceps_states[8]; //Aantal states van het filter float xtf; //Gefilterde EMG waarde triceps float xtfmax = 0; //Maximale gefilterde EMG waarde triceps float xbt; //Thresholdwaarde EMG biceps float xtt; //Thresholdwaarde EMG triceps volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true { regelaar_ticker_flag = true; } volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true { regelaar_EMG_flag = true; } 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 *in = min; } 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 *in = *in; } } 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; //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); } else { //Anders richting nul dir_motor_arm1.write(1); } 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() //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; //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); } 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 } 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 } void filter_triceps() //Filtert het EMG signaal van de triceps { arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1); //Highpass filter voor het EMG signaal van de triceps, filtert de xt en schrijft het over in de xtf, 1 EMG waarde filteren per keer arm_biquad_cascade_df1_f32(¬ch_triceps, &xtf, &xtf, 1); //Notch filter voor het EMG signaal van de triceps, filtert de xtf uit de highpass en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer xtf = fabs(xtf); //Rectifier, schrijft de absolute waarde van de xtf uit de notch opnieuw in de xtf, fabs omdat het een float is arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1); //Lowpass filter voor het EMG signaal van de triceps, filtert de xtf uit de rectifier en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer pc.printf("xtf is %f\n\r", xtf); //De gefilterde EMG waarde van de triceps naar pc sturen } void 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 xt = EMG_tri.read(); //EMG signaal triceps uitlezen filter_triceps(); //EMG signaal triceps filteren } int main() //Main script { 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 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 lcd.locate(0,0); //Zet de tekst op de eerste regel lcd.printf(" BMT K9 GR. 4 "); //Tekst op LCD scherm lcd.locate(0,1); //Zet de tekst op de tweede regel lcd.printf("HOI! IK BEN PIPO"); //Tekst op LCD scherm wait(2); //Twee seconden wachten lcd.cls(); //Maak LCD scherm leeg pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten state = KALIBRATIE_ARM1; //Door naar de volgende state break; //Stopt acties in deze case } case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM1 naar pc sturen wait(1); //Een seconde wachten ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken while(state == KALIBRATIE_ARM1) { while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan referentie_arm1 = referentie_arm1 + 180.0/200.0; //Referentie loopt in één seconde op tot het gewenste aantal pulsen 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 } 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(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; //Referentie loopt in één seconde op tot het gewenste aantal pulsen 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 break; //Stopt acties in deze case } case EMG_KALIBRATIE_BICEPS: { //Kalibratie biceps voor bepalen threshold pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm lcd.locate(0,1); //Zet tekst op tweede regel lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm 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 arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 2, lowpass_biceps_const, lowpass_biceps_states); //Lowpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt 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 xb = EMG_bi.read(); //EMG meten van biceps 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 lcd.locate(0,0); //Zet de tekst op de eerste regel lcd.printf(" 5 "); //Tekst op LCD scherm pc.printf("4"); //Controle naar pc sturen } if(int(biceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten lcd.locate(0,0); //Zet de tekst op de eerste regel lcd.printf(" 4 "); //Tekst op LCD scherm pc.printf("3"); //Controle naar pc sturen } if(int(biceps_kalibratie.read()) == 2) { //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 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 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 xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax } } xbt = xbfmax * 0.80; //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: { //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); //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 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(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan xt = EMG_tri.read(); //EMG meten van ticeps 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 lcd.locate(0,0); //Zet de tekst op de eerste regel lcd.printf(" 5 "); //Tekst op LCD scherm pc.printf("4"); //Controle naar pc sturen } if(int(triceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten lcd.locate(0,0); //Zet de tekst op de eerste regel lcd.printf(" 4 "); //Tekst op LCD scherm pc.printf("3"); //Controle naar pc sturen } if(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 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 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 xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax } } xtt = xtfmax * 0.80; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten 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: { //Eerste keuze maken voor doel 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 EMG.start(); //Timer aanzetten voor EMG meten 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 van biceps en triceps } while(state == START) { while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan xb = EMG_bi.read(); //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(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 state = T; //Ga door naar state T } } break; //Stopt met de acties in deze case } 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.reset(); EMG.start(); //Timer aanzetten voor EMG meten 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 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 van biceps en triceps 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 state = BT; //Ga door naar state BT } } break; //Stop met alle acties in deze case } 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.reset(); EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = TT; //Ga door naar state TT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = BBT; //Ga door naar state BBT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 van biceps en triceps } 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 EMG_meten(); 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 state = BTT; //Ga door naar state BTT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = TBT; //Ga door naar state TBT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = TTT; //Ga door naar state TTT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = BBBT; //Ga door naar state BBBT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = BBTT; //Ga door naar state BBTT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = BTBT; //Ga door naar state BTBT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = BTTT; //Ga door naar state BTTT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 van biceps en triceps 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 state = TBBT; //Ga door naar state TBBT } } break; //Stop met alle acties in deze case } 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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 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 } if(xtf >= 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: { //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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 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 } if(xtf >= 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: { //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(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee 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 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 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 } if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold state = TTTT; //Ga door naar state TTTT } } break; //Stop met alle acties in deze case } case BBBB: { //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) { 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; //Referentie arm 1 loopt af in een seconden naar de gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } if(puls_motor_arm1.getPosition() <= -84) { referentie_arm1 = -84; } while(puls_motor_arm2.getPosition() < 211) { referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar de gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.detach(); pwm_to_motor_arm2 = 1; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(0); while(puls_motor_arm2.getPosition() > 36) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen } pwm_to_motor_arm2 = 0.5; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(1); while(puls_motor_arm2.getPosition() <= 211) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); pc.printf("staat stil"); state = THUISPOSITIE_RECHTS; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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 wait(1); //Een seconde wachten lcd.cls(); //LCD scherm leegmaken while(state == BBBT) { lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm wait(1); //Een seconde wachten state = WACHT; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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) { 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; //Referentie arm 1 loopt af in een seconde naar gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } if(puls_motor_arm1.getPosition() <= -84) { referentie_arm1 = -84; } while(puls_motor_arm2.getPosition() < 211) { referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.detach(); pwm_to_motor_arm2 = 1; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(0); while(puls_motor_arm2.getPosition() > 36) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen } pwm_to_motor_arm2 = 0.5; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(1); while(puls_motor_arm2.getPosition() <= 211) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); state = THUISPOSITIE_RECHTS; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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) { 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 - 259.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } if(puls_motor_arm1.getPosition() <= -84) { referentie_arm1 = -84; } while(puls_motor_arm2.getPosition() < 211) { referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.detach(); pwm_to_motor_arm2 = 1; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(0); while(puls_motor_arm2.getPosition() > 36) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen } pwm_to_motor_arm2 = 0.5; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(1); while(puls_motor_arm2.getPosition() <= 211) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); pc.printf("staat stil\n\r"); //Staat stil naar pc sturen state = THUISPOSITIE_RECHTS; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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 wait(1); //Een seconde wachten lcd.cls(); //LCD scherm leegmaken while(state == BTBB) { lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm wait(1); //Een seconde wachten state = WACHT; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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 wait(1); //Een seconde wachten lcd.cls(); //LCD scherm leegmaken while(state == BTBT) { lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm wait(1); //Een seconde wachten state = WACHT; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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 wait(1); //Een seconde wachten lcd.cls(); //LCD scherm leegmaken while(state == BTTB) { lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm wait(1); //Een seconde wachten state = WACHT; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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 wait(1); //Een seconde wachten lcd.cls(); //LCD scherm leegmaken while(state == BTTT) { lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm wait(1); //Een seconde wachten state = WACHT; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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) { 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; //Referentie arm 1 loopt af in een seconde naar gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } if(puls_motor_arm1.getPosition() <= 48) { referentie_arm1 = 48; } while(puls_motor_arm2.getPosition() < 167) { referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.detach(); pwm_to_motor_arm2 = 1; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(0); while(puls_motor_arm2.getPosition() > -8) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen } pwm_to_motor_arm2 = 0.5; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(1); while(puls_motor_arm2.getPosition() <= 167) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); pc.printf("staat stil\n\r"); //Staat stil naar pc sturen state = THUISPOSITIE_MIDDEN; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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 wait(1); //Een seconde wachten lcd.cls(); //LCD scherm leegmaken while(state == TBBT) { lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm wait(1); //Een seconde wachten state = WACHT; //Door naar de volgende state } 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; //Referentie arm 1 loopt af in een seconde naar gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } if(puls_motor_arm1.getPosition() <= 48) { referentie_arm1 = 48; } while(puls_motor_arm2.getPosition() < 167) { referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.detach(); pwm_to_motor_arm2 = 1; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(0); while(puls_motor_arm2.getPosition() > -8) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen } pwm_to_motor_arm2 = 0.5; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(1); while(puls_motor_arm2.getPosition() <= 167) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); pc.printf("staat stil\n\r"); //Staat stil naar pc sturen state = THUISPOSITIE_MIDDEN; //Door naar de volgende state } 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; //Referentie arm 1 loopt af in een seconde naar gewenste positie pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } if(puls_motor_arm1.getPosition() <= 48) { referentie_arm1 = 48; } while(puls_motor_arm2.getPosition() < 167) { referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.detach(); pwm_to_motor_arm2 = 1; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(0); while(puls_motor_arm2.getPosition() > -8) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen } pwm_to_motor_arm2 = 0.5; pwm_motor_arm2.write(pwm_to_motor_arm2); dir_motor_arm2.write(1); while(puls_motor_arm2.getPosition() <= 167) { pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); pc.printf("staat stil\n\r"); //Staat stil naar pc sturen state = THUISPOSITIE_MIDDEN; //Door naar de volgende state } 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) { 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 = -0.5 * 100000 * t * t; //Referentie arm 2 loopt parabolisch af t = t + 0.005; //Tijd loopt op met 0.005 per flag pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen if(puls_motor_arm2.getPosition() <= -175) { wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 0) { referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } if(puls_motor_arm2.getPosition() >= 0) { pc.printf("staat stil\n\r"); //Staat stil naar pc sturen state = WACHT; //Door naar de volgende state } } } break; //Stop met alle acties in deze case } 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 wait(1); //Een seconde wachten lcd.cls(); //LCD scherm leegmaken while(state == TTBT) { lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm wait(1); //Een seconde wachten state = WACHT; //Door naar de volgende state } break; //Stop met alle acties in deze case } 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) { 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 = -0.5 * 150000 * t * t; //Referentie arm 2 loopt parabolisch af t = t + 0.005; //Tijd loopt op met 0.005 per flag pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen if(puls_motor_arm2.getPosition() <= -175) { wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 0) { referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } if(puls_motor_arm2.getPosition() >= 0) { pc.printf("staat stil\n\r"); //Staat stil naar pc sturen state = WACHT; //Door naar de volgende state } } } break; //Stop met alle acties in deze case } 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 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 referentie_arm2 = -0.5 * 200000 * t * t; //Referentie arm 2 loopt parabolisch af t = t + 0.005; //Tijd loopt op met 0.005 per flag pwm_motor_arm1 = 0.075; pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen pc.printf("t is %f\n\r", t); //T naar pc sturen if(puls_motor_arm2.getPosition() <= -175) { wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 0) { referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } if(puls_motor_arm2.getPosition() >= 0) { pc.printf("staat stil\n\r"); //Staat stil naar pc sturen state = WACHT; //Door naar de volgende state } } } break; } case THUISPOSITIE_MIDDEN: { //Terug naar gekalibreerde positie 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; //Referentie arm 2 loopt af in een seconde naar gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } 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; //Referentie arm 1 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen } state = WACHT; //Door naar de volgende state break; } case THUISPOSITIE_RECHTS: { //Terug naar gekalibreerde positie 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; //Referentie arm 2 loopt af in een seconde naar gewenste waarde pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } while(puls_motor_arm1.getPosition() < 175) { 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 + 259.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen } state = WACHT; break; } case WACHT: { //Even wachten en weer terug naar start lcd.locate(0,0); //Zet tekst op eerste regel lcd.printf(" WACHT "); //Tekst op LCD scherm wait(5); //Vijf seconden wachten state = START; //Terug naar state START break; } 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 } } } }