171 lines
4.3 KiB
C
171 lines
4.3 KiB
C
#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;
|
|
}
|