From 98c623dc8969151c0872a2d102d486f9a735d614 Mon Sep 17 00:00:00 2001
From: neoraider <devnull@localhost>
Date: Tue, 3 Apr 2007 12:47:03 +0000
Subject: Neues Rescue-Programm angefangen

---
 Robocup.c | 171 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 171 insertions(+)
 create mode 100644 Robocup.c

(limited to 'Robocup.c')

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;
+}
-- 
cgit v1.2.3