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:
- 24:a1fdc830f96c
- Parent:
- 23:5267c928ae2b
- Child:
- 25:71e52c56be13
--- a/main.cpp Sun Nov 02 17:08:31 2014 +0000
+++ b/main.cpp Sun Nov 02 19:42:31 2014 +0000
@@ -1043,201 +1043,198 @@
if(puls_motor_arm2.getPosition() <= -8) {
wait(1);
- while(puls_motor_arm2.getPosition() > -100) {
+ while(puls_motor_arm2.getPosition() < 167) {
referentie_arm2 = referentie_arm2 + 167.0/200.0;
pc.printf("referentie arm 2 %f ", referentie_arm2);
pc.printf("get position %d ", puls_motor_arm2.getPosition());
pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
-
-
-
- if(puls_motor_arm2.getPosition() >= 167) {
- pc.printf("staat stil\n\r");
- state = WACHT;
- }
-
- }
- }
- }
- break; //Stop met alle acties in deze case
- }
-
- case TTBB: { //Motoraansturing voor richten op doel linksonder
- lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" TTBB "); //Tekst op LCD scherm
- pc.printf("TTBB\n\r"); //Controle naar pc sturen
-
- while(state == TTBB) {
- //Motoractie
- while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
- regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
-
- //Positie arm 1 is goed
- //Snelheid arm 2
-
- referentie_arm2 = 0.5 * 200000 * t * t;
- t = t + 0.005;
-
- pc.printf("referentie arm 2 %f ", referentie_arm2);
- pc.printf("get position %d ", puls_motor_arm2.getPosition());
- pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
- pc.printf("t is %f\n\r", t);
-
- if(puls_motor_arm2.getPosition() >= 175) {
- wait(1);
- while(puls_motor_arm2.getPosition() > 0) {
- referentie_arm2 = referentie_arm2 - 175.0/200.0;
-
-
- pc.printf("referentie arm 2 %f ", referentie_arm2);
- pc.printf("get position %d ", puls_motor_arm2.getPosition());
- pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
}
-
- if(puls_motor_arm2.getPosition() <= 0) {
- pc.printf("staat stil\n\r");
- state = WACHT;
- }
-
- }
- }
- break; //Stop met alle acties in deze case
- }
-
- case TTBT: { //Motoraansturing voor terug naar kalibratiepositie
- lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" TTBT "); //Tekst op LCD scherm
- pc.printf("TTBT\n\r"); //Controle naar pc sturen
-
- while(state == TTBT) {
- //Motoractie
- lcd.printf(" GEEN DOEL ");
- wait(1);
- state = WACHT;
- }
- break; //Stop met alle acties in deze case
- }
-
- case TTTB: { //Motoraansturing voor richten op doel linksmidden
- lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" TTTB "); //Tekst op LCD scherm
- pc.printf("TTTB\n\r"); //Controle naar pc sturen
-
- while(state == TTTB) {
- //Motoractie
-
- while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
- regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
-
- //Positie arm 1 is goed
- //Snelheid arm 2
-
- referentie_arm2 = 0.5 * 400000 * t * t;
- t = t + 0.005;
-
- pc.printf("referentie arm 2 %f ", referentie_arm2);
- pc.printf("get position %d ", puls_motor_arm2.getPosition());
- pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
- pc.printf("t is %f\n\r", t);
-
- if(puls_motor_arm2.getPosition() >= 175) {
- wait(1);
- while(puls_motor_arm2.getPosition() > 0) {
- referentie_arm2 = referentie_arm2 - 175.0/200.0;
-
-
- pc.printf("referentie arm 2 %f ", referentie_arm2);
- pc.printf("get position %d ", puls_motor_arm2.getPosition());
- pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
- }
-
-
- if(puls_motor_arm2.getPosition() <= 0) {
+ if(puls_motor_arm2.getPosition() >= 167) {
pc.printf("staat stil\n\r");
state = WACHT;
}
-
- }
- }
-
- break; //Stop met alle acties in deze case
- }
-
-
- case TTTT: { //Motoraansturing voor richten op 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
-
- //Positie arm 1 is goed
- //Snelheid arm 2
-
- referentie_arm2 = 0.5 * 600000 * t * t;
- t = t + 0.005;
-
- pc.printf("referentie arm 2 %f ", referentie_arm2);
- pc.printf("get position %d ", puls_motor_arm2.getPosition());
- pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
- pc.printf("t is %f\n\r", t);
-
- if(puls_motor_arm2.getPosition() >= 175) {
- wait(1);
- while(puls_motor_arm2.getPosition() > 0) {
- referentie_arm2 = referentie_arm2 - 175.0/200.0;
-
-
- pc.printf("referentie arm 2 %f ", referentie_arm2);
- pc.printf("get position %d ", puls_motor_arm2.getPosition());
- pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
- }
-
-
- if(puls_motor_arm2.getPosition() <= 0) {
- pc.printf("staat stil\n\r");
- state = WACHT;
- }
-
}
}
}
+ break; //Stop met alle acties in deze case
+ }
- case THUISPOSITIE_MIDDEN: {
- while(puls_motor_arm2.getPosition() > 123) {
- referentie_arm2 = referentie_arm2 - 44.0/200.0;
+ case TTBB: { //Motoraansturing voor richten op doel linksonder
+ lcd.locate(0,0); //Zet tekst op eerste regel
+ lcd.printf(" TTBB "); //Tekst op LCD scherm
+ pc.printf("TTBB\n\r"); //Controle naar pc sturen
+
+ while(state == TTBB) {
+ //Motoractie
+ while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ //Positie arm 1 is goed
+ //Snelheid arm 2
+
+ referentie_arm2 = 0.5 * 100000 * t * t;
+ t = t + 0.005;
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+ pc.printf("t is %f\n\r", t);
+
+ if(puls_motor_arm2.getPosition() >= 175) {
+ wait(1);
+ while(puls_motor_arm2.getPosition() > 0) {
+ referentie_arm2 = referentie_arm2 - 175.0/200.0;
+
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+ }
+
+
+ if(puls_motor_arm2.getPosition() <= 0) {
+ pc.printf("staat stil\n\r");
+ state = WACHT;
+ }
+
}
+ }
+ break; //Stop met alle acties in deze case
+ }
- while(puls_motor_arm1.getPosition() < 180) {
- referentie_arm1 = referentie_arm1 + 132.0/200.0;
+ case TTBT: { //Motoraansturing voor terug naar kalibratiepositie
+ lcd.locate(0,0); //Zet tekst op eerste regel
+ lcd.printf(" TTBT "); //Tekst op LCD scherm
+ pc.printf("TTBT\n\r"); //Controle naar pc sturen
+
+ while(state == TTBT) {
+ //Motoractie
+ lcd.printf(" GEEN DOEL ");
+ wait(1);
+ state = WACHT;
+ }
+ break; //Stop met alle acties in deze case
+ }
+
+ case TTTB: { //Motoraansturing voor richten op doel linksmidden
+ lcd.locate(0,0); //Zet tekst op eerste regel
+ lcd.printf(" TTTB "); //Tekst op LCD scherm
+ pc.printf("TTTB\n\r"); //Controle naar pc sturen
+
+ while(state == TTTB) {
+ //Motoractie
+
+ while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ //Positie arm 1 is goed
+ //Snelheid arm 2
+
+ referentie_arm2 = 0.5 * 150000 * t * t;
+ t = t + 0.005;
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+ pc.printf("t is %f\n\r", t);
+
+ if(puls_motor_arm2.getPosition() >= 175) {
+ wait(1);
+ while(puls_motor_arm2.getPosition() > 0) {
+ referentie_arm2 = referentie_arm2 - 175.0/200.0;
+
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+ }
+
+
+ if(puls_motor_arm2.getPosition() <= 0) {
+ pc.printf("staat stil\n\r");
+ state = WACHT;
+ }
+
}
-
- state = WACHT;
- break;
}
- case WACHT: {
- lcd.printf(" WACHT ");
- wait(5);
- state = START;
+ break; //Stop met alle acties in deze case
+ }
+
+
+ case TTTT: { //Motoraansturing voor richten op 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
+
+ //Positie arm 1 is goed
+ //Snelheid arm 2
+
+ referentie_arm2 = 0.5 * 200000 * t * t;
+ t = t + 0.005;
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+ pc.printf("t is %f\n\r", t);
+
+ if(puls_motor_arm2.getPosition() >= 175) {
+ wait(1);
+ while(puls_motor_arm2.getPosition() > 0) {
+ referentie_arm2 = referentie_arm2 - 175.0/200.0;
+
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+ }
+
+
+ if(puls_motor_arm2.getPosition() <= 0) {
+ pc.printf("staat stil\n\r");
+ state = WACHT;
+ }
+
+ }
+ }
+ }
+
+ case THUISPOSITIE_MIDDEN: {
+ while(puls_motor_arm2.getPosition() > 123) {
+ referentie_arm2 = referentie_arm2 - 44.0/200.0;
+ }
+
+ while(puls_motor_arm1.getPosition() < 180) {
+ referentie_arm1 = referentie_arm1 + 132.0/200.0;
+ }
+
+ state = WACHT;
+ break;
+ }
+
+ case WACHT: {
+ lcd.printf(" WACHT ");
+ wait(5);
+ state = START;
- default: {
- //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
- state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
- }
+ default: {
+ //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
+ state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
+ }
- pc.printf("state: %u\n\r",state);
- }
- //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+ pc.printf("state: %u\n\r",state);
}
+ //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
}
}
-}
\ No newline at end of file
+}