diff options
Diffstat (limited to 'Robocup.c')
| -rw-r--r-- | Robocup.c | 171 |
1 files changed, 171 insertions, 0 deletions
diff --git a/Robocup.c b/Robocup.c new file mode 100644 index 0000000..08258df --- /dev/null +++ b/Robocup.c @@ -0,0 +1,171 @@ +#include "Global.h"
+#include "Navigation.h"
+#include "Sensors.h"
+#include "qfixSoccerBoard.h"
+
+
+
+void OpferSignal(SoccerBoard *board) {
+ board->ledOn(0);
+ board->beep(OPFER_BEEP);
+ board->sleep(OPFER_WAIT);
+ board->ledOff(0);
+ board->beepOff();
+}
+
+
+int main() {
+ int status = STATUS_OK;
+ int linie = 0, last_linie = 0;
+ SoccerBoard board;
+ Sensors sensors(&board);
+ Navigation nav(&board);
+
+ do {
+ sensors.update();
+ } while(!sensors.kalibrieren());
+
+ board.ledOn(0);
+ board.waitForButton(0);
+ board.ledOff(0);
+ board.sleep(4000);
+
+ nav.start();
+
+ while(true) {
+ sensors.update();
+ sensors.auswerten();
+
+ last_linie = linie;
+ linie = sensors.getLinie();
+
+ switch(status) {
+ case STATUS_OK:
+ if(linie == 0) sensors.kalibrieren();
+
+ if(sensors.getOpfer() && board.timer()) {
+ nav.stop();
+ OpferSignal(&board);
+ nav.start();
+ board.setTimer(OPFER_TIMER);
+ break;
+ }
+ + if(sensors.hindernisV()) { + nav.setSpeed(SPEED_RUECK); + nav.setRichtung(0);
+ board.sleep(HINDERNIS_SLEEP); + + nav.setSpeed(DEFAULT_SPEED); + + if(sensors.getDsVL() > sensors.getDsVR()) {
+ // Hindernis eher links -> wir fahren rechts rum + nav.setRichtung(2);
+ status = STATUS_HINDERNIS_VOR_L; // status variable setzen + }
+ else {
+ // Hindernis eher rechts -> wir fahren links rum + nav.setRichtung(-2);
+ status = STATUS_HINDERNIS_VOR_R; // status variable setzen + } + + break; + } +
+ switch(linie) {
+ case LINIE_FEHLER:
+ break;
+ case LINIE_WEISS:
+ /*if(abs(last_linie) == 2) break;
+
+ nav.setRichtung(0);
+ status = STATUS_WEISS;*/
+
+ break;
+ default:
+ nav.setRichtung(linie);
+ } + + break;
+ case STATUS_WEISS:
+ if(sensors.getOpfer() && board.timer()) {
+ nav.stop();
+ OpferSignal(&board);
+ nav.start();
+ board.setTimer(OPFER_TIMER);
+ continue;
+ }
+
+ if(linie != LINIE_WEISS) status = STATUS_OK; + break; + case STATUS_HINDERNIS_VOR_L: + if(sensors.hindernisL() && linie == LINIE_WEISS) status = STATUS_HINDERNIS_L; + break; + case STATUS_HINDERNIS_VOR_R: + if(sensors.hindernisR() && linie == LINIE_WEISS) status = STATUS_HINDERNIS_R; + break; + case STATUS_HINDERNIS_L: + if(sensors.hindernisVL()) { + nav.setSpeed(SPEED_RUECK); + nav.setRichtung(0);
+ board.sleep(HINDERNIS_SLEEP); + + nav.setSpeed(DEFAULT_SPEED); + nav.setRichtung(2); + status = STATUS_HINDERNIS_VOR_L; + } + else if(linie == LINIE_WEISS) { + if(sensors.getDsL() > HINDERNIS_SOLLWERT + HINDERNIS_ABW)
+ nav.setRichtung(2);
+ else if(sensors.getDsL() < HINDERNIS_SOLLWERT - HINDERNIS_ABW)
+ nav.setRichtung(-2);
+ else
+ nav.setRichtung(0); + } + else { + status = STATUS_HINDERNIS_WAR_L;
+ nav.setRichtung(0); + } + break; + case STATUS_HINDERNIS_R: + if(sensors.hindernisVR()) { + nav.setSpeed(SPEED_RUECK); + nav.setRichtung(0);
+ board.sleep(HINDERNIS_SLEEP); + + nav.setSpeed(DEFAULT_SPEED); + nav.setRichtung(-2); + status = STATUS_HINDERNIS_VOR_R; + } + else if(linie == LINIE_WEISS) { + if(sensors.getDsR() > HINDERNIS_SOLLWERT + HINDERNIS_ABW)
+ nav.setRichtung(-2);
+ else if(sensors.getDsR() < HINDERNIS_SOLLWERT - HINDERNIS_ABW)
+ nav.setRichtung(2);
+ else
+ nav.setRichtung(0); + } + else { + status = STATUS_HINDERNIS_WAR_R;
+ nav.setRichtung(0); + } + break; + case STATUS_HINDERNIS_WAR_L: + if(linie != LINIE_WEISS) break; + + board.sleep(40);
+ status = STATUS_OK;
+ nav.setRichtung(2); + + break; + case STATUS_HINDERNIS_WAR_R: + if(linie != LINIE_WEISS) break; + + board.sleep(40);
+ status = STATUS_OK;
+ nav.setRichtung(-2);
+ }
+ }
+
+ return 0;
+}
|