summaryrefslogtreecommitdiffstats
path: root/Robocup.c
diff options
context:
space:
mode:
Diffstat (limited to 'Robocup.c')
-rw-r--r--Robocup.c171
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;
+}