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:
- 26:438a498e5526
- Parent:
- 25:71e52c56be13
- Child:
- 27:5ac1fc1005d7
--- a/main.cpp Mon Nov 03 11:13:33 2014 +0000 +++ b/main.cpp Mon Nov 03 13:00:39 2014 +0000 @@ -475,7 +475,7 @@ EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -506,7 +506,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -537,7 +537,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -568,7 +568,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -599,7 +599,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -630,7 +630,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -662,7 +662,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -693,7 +693,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -724,7 +724,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -755,7 +755,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -786,7 +786,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -799,10 +799,10 @@ 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 = TBTB; //Ga door naar state TBTB } - 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 = TBTT; //Ga door naar state TBTT } } @@ -817,7 +817,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -830,10 +830,10 @@ 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 = TTBB; //Ga door naar state TTBB } - 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 = TTBT; //Ga door naar state TTBT } } @@ -848,7 +848,7 @@ EMG.reset(); //Timer resetten EMG.start(); //Timer aanzetten voor EMG meten - while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 @@ -861,10 +861,10 @@ 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 = TTTB; //Ga door naar state TTTB } - 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 = TTTT; //Ga door naar state TTTT } } @@ -881,45 +881,43 @@ 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); + 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 } - 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); + 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 } - pc.printf("arm2 op positie"); while(puls_motor_arm2.getPosition() > 36) { - referentie_arm2 = -0.5 * 100000 * t * t; - t = t + 0.005; + 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); - 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("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", 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() <= 36) { - wait(1); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 211) { - referentie_arm2 = referentie_arm2 + 175.0/200.0; + referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar de gewenste waarde - 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); + 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 } if(puls_motor_arm2.getPosition() >= 211) { - pc.printf("staat stil\n\r"); - state = THUISPOSITIE_RECHTS; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = THUISPOSITIE_RECHTS; //Door naar de volgende state } } } @@ -930,11 +928,14 @@ 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.printf(" GEEN DOEL "); - wait(1); - state = WACHT; + 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 } @@ -949,47 +950,42 @@ 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); + 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 } - 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); + 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 } - pc.printf("arm2 op positie"); - - while(puls_motor_arm2.getPosition() > 36) { - - referentie_arm2 = -0.5 * 150000 * t * t; - t = t + 0.005; + 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); - 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("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", 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() <= 36) { - wait(1); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 211) { - referentie_arm2 = referentie_arm2 + 175.0/200.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); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + 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 } if(puls_motor_arm2.getPosition() >= 211) { - pc.printf("staat stil\n\r"); - state = THUISPOSITIE_RECHTS; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = THUISPOSITIE_RECHTS; //Door naar de volgende state } } } @@ -1006,45 +1002,42 @@ 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); + 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 } - 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); + 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 } - pc.printf("arm2 op positie"); while(puls_motor_arm2.getPosition() > 36) { - - referentie_arm2 = -0.5 * 200000 * t * t; - t = t + 0.005; + referentie_arm2 = -0.5 * 200000 * t * t; //Referentie arm 2 loopt parabolisch af + t = t + 0.005; //Tijd loopt op met 0.005 iedere flag - 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("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", 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() <= 36) { - wait(1); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 211) { - referentie_arm2 = referentie_arm2 + 175.0/200.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); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + 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 } if(puls_motor_arm2.getPosition() >= 211) { - pc.printf("staat stil\n\r"); - state = THUISPOSITIE_RECHTS; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = THUISPOSITIE_RECHTS; //Door naar de volgende state } } } @@ -1055,11 +1048,14 @@ 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.printf(" GEEN DOEL "); - wait(1); - state = WACHT; + 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 } @@ -1068,11 +1064,14 @@ 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.printf(" GEEN DOEL "); - wait(1); - state = WACHT; + 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 } @@ -1081,11 +1080,14 @@ 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.printf(" GEEN DOEL "); - wait(1); - state = WACHT; + 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 } @@ -1094,11 +1096,14 @@ 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.printf(" GEEN DOEL "); - wait(1); - state = WACHT; + 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 } @@ -1113,45 +1118,42 @@ 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); + 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 } - 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); + 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 } - pc.printf("arm2 op positie"); while(puls_motor_arm2.getPosition() > -8) { - - referentie_arm2 = -0.5 * 100000 * t * t; - t = t + 0.005; + 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); - 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("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", 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() <= -8) { - wait(1); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 167) { - referentie_arm2 = referentie_arm2 + 175.0/200.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); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + 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 } if(puls_motor_arm2.getPosition() >= 167) { - pc.printf("staat stil\n\r"); - state = THUISPOSITIE_MIDDEN; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = THUISPOSITIE_MIDDEN; //Door naar de volgende state } } } @@ -1162,11 +1164,14 @@ 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.printf(" GEEN DOEL "); - wait(1); - state = WACHT; + 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 } @@ -1181,45 +1186,42 @@ 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); + 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 } - 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); + 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 } - pc.printf("arm2 op positie"); while(puls_motor_arm2.getPosition() > -8) { - - referentie_arm2 = -0.5 * 150000 * t * t; - t = t + 0.005; + referentie_arm2 = -0.5 * 150000 * t * t; //Referentie arm 2 loopt parabolisch op + t = t + 0.005; //Tijd loopt op met 0.005 per flag - 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("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", 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() <= -8) { - wait(1); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 167) { - referentie_arm2 = referentie_arm2 + 175.0/200.0; + referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie - 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); + 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 } if(puls_motor_arm2.getPosition() >= 167) { - pc.printf("staat stil\n\r"); - state = THUISPOSITIE_MIDDEN; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = THUISPOSITIE_MIDDEN; //Door naar de volgende state } } } @@ -1236,45 +1238,42 @@ 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); + 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 } - 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); + 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 } - pc.printf("arm2 op positie"); while(puls_motor_arm2.getPosition() > -8) { - - referentie_arm2 = -0.5 * 200000 * t * t; - t = t + 0.005; + 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 - 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("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", 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() <= -8) { - wait(1); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 167) { - referentie_arm2 = referentie_arm2 + 175.0/200.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); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + 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 } if(puls_motor_arm2.getPosition() >= 167) { - pc.printf("staat stil\n\r"); - state = THUISPOSITIE_MIDDEN; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = THUISPOSITIE_MIDDEN; //Door naar de volgende state } } } @@ -1290,27 +1289,27 @@ 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; - t = t + 0.005; + 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); - 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("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", 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); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 0) { - referentie_arm2 = referentie_arm2 + 175.0/200.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); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + 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 } if(puls_motor_arm2.getPosition() >= 0) { - pc.printf("staat stil\n\r"); - state = WACHT; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = WACHT; //Door naar de volgende state } } } @@ -1321,11 +1320,14 @@ 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.printf(" GEEN DOEL "); - wait(1); - state = WACHT; + 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 } @@ -1340,27 +1342,27 @@ 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; - t = t + 0.005; + 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); - 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("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", 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); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 0) { - referentie_arm2 = referentie_arm2 + 175.0/200.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); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + 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 } if(puls_motor_arm2.getPosition() >= 0) { - pc.printf("staat stil\n\r"); - state = WACHT; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = WACHT; //Door naar de volgende state } } } @@ -1377,96 +1379,90 @@ 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; - t = t + 0.005; + 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 - 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("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", 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); + wait(1); //Een seconde wachten while(puls_motor_arm2.getPosition() < 0) { - referentie_arm2 = referentie_arm2 + 175.0/200.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); - pc.printf("get position %d ", puls_motor_arm2.getPosition()); - pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); + 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 } if(puls_motor_arm2.getPosition() >= 0) { - pc.printf("staat stil\n\r"); - state = WACHT; + pc.printf("staat stil\n\r"); //Staat stil naar pc sturen + state = WACHT; //Door naar de volgende state } } } } - case THUISPOSITIE_MIDDEN: { + 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; - 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); + 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 %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; - 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); - + 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; + state = WACHT; //Door naar de volgende state break; } - case THUISPOSITIE_RECHTS: { - pc.printf("thuispositie rechts"); + 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; - 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); - + 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() < 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); + referentie_arm1 = referentie_arm1 + 264.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 } } - case WACHT: { - lcd.printf(" WACHT "); - wait(5); - state = START; + 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(3); //Drie seconden wachten + state = START; //Terug naar 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 - } - - pc.printf("state: %u\n\r",state); + default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case + state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan } - //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) } } } +