Janine Klein Rot
2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
Diff: main.cpp
- Revision:
- 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)
}
}
}
+