summaryrefslogtreecommitdiffstats
path: root/main.cpp
diff options
context:
space:
mode:
authorneoraider <devnull@localhost>2007-04-19 01:00:05 +0200
committerneoraider <devnull@localhost>2007-04-19 01:00:05 +0200
commite7eeeb0d4e825a175c67cd1c0256c91362517ebb (patch)
tree32cd6997837c9decb909dc36824ea7abc896c88e /main.cpp
parent847645b3cc8dd32b51f9c0a42d0b4308fa40e4a5 (diff)
downloadrc2007-rescue-e7eeeb0d4e825a175c67cd1c0256c91362517ebb.tar
rc2007-rescue-e7eeeb0d4e825a175c67cd1c0256c91362517ebb.zip
Entfernungssensoren in Ordnung gebracht, Hindernisumfahrung implementiert.
Diffstat (limited to 'main.cpp')
-rw-r--r--main.cpp66
1 files changed, 50 insertions, 16 deletions
diff --git a/main.cpp b/main.cpp
index 0da17a9..8fc011b 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,6 +1,7 @@
#include "avr.h"
#include "global.h"
#include "hardware.h"
+#include "timer.h"
#include "util.h"
#include "LineSensor.h"
#include "LineSensorArray.h"
@@ -9,15 +10,6 @@
#include "Srf10.h"
-
-
-
-
-
-
-
-
-
static void delay() {
for(unsigned long i = 0; i < 25000; i++)
asm("nop");
@@ -45,14 +37,16 @@ int main() {
lineSensorArray->setMinimumDifference(CAL_MIN_DIFF);
lineSensorArray->setMaximumWhiteDifference(CAL_MAX_WHITE_DIFF);
- //Srf10 *srf10Right = new Srf10(0xE0);
+ Srf10 *srf10Right = new Srf10(0xE0);
Srf10 *srf10Left = new Srf10(0xE2);
- //srf10Right->setUnit(Centimeters);
- srf10Left->setUnit(Centimeters);
+ srf10Right->setUnit(Centimeters);
+ srf10Right->setGain(DISTANCE_GAIN);
- //Srf10 *srf10Last = srf10Left;
+ srf10Left->setUnit(Centimeters);
+ srf10Left->setGain(DISTANCE_GAIN);
+ Srf10 *srf10Last = srf10Left;
do {
lineSensorArray->update();
@@ -63,8 +57,8 @@ int main() {
while(true) {
lineSensorArray->update();
- //srf10Last = (srf10Last==srf10Left)?srf10Right:srf10Left;
- //srf10Last->updateDistance();
+ srf10Last = (srf10Last==srf10Left)?srf10Right:srf10Left;
+ srf10Last->updateDistance();
switch(status) {
case Ok:
@@ -73,7 +67,13 @@ int main() {
// TODO: victims!!
- // TODO: debris!!
+ if(srf10Last->hasDistance() && srf10Last->getDistance() <= DISTANCE_RECOG) {
+ navigation->setSpeed(DEFAULT_SPEED);
+ navigation->setDirection(90.0);
+
+ status = Debris;
+ break;
+ }
if(lineSensorArray->getSharpness() < 0.0)
break;
@@ -121,7 +121,41 @@ int main() {
case White:
break;
case Debris:
+ case DebrisWhite:
+ if(srf10Right->getDistance() < srf10Left->getDistance())
+ navigation->setSpin(DISTANCE_SPIN);
+ else if(srf10Right->getDistance() > srf10Left->getDistance())
+ navigation->setSpin(-DISTANCE_SPIN);
+ else
+ navigation->setSpin(0);
+
+ if((srf10Right->getDistance()+srf10Left->getDistance())/2 > DISTANCE_MAX)
+ navigation->setDirection(90.0-DISTANCE_ANGLE);
+ else if((srf10Right->getDistance()+srf10Left->getDistance())/2 < DISTANCE_MIN)
+ navigation->setDirection(90.0+DISTANCE_ANGLE);
+ else
+ navigation->setDirection(90.0);
+
+ if(status == Debris && lineSensorArray->isSensorWhite(2))
+ status = DebrisWhite;
+ else if(status == DebrisWhite && lineSensorArray->isSensorBlack(2)) {
+ navigation->setSpeed(0.0);
+ navigation->setSpin(TURN_SPIN);
+ status = Turn;
+ }
+ break;
+ case Turn:
+ if(lineSensorArray->isSensorWhite(2))
+ status = TurnWhite;
break;
+ case TurnWhite:
+ if(lineSensorArray->isSensorBlack(2)) {
+ navigation->setSpeed(DEFAULT_SPEED);
+ navigation->setDirection(0.0);
+ navigation->setSpin(0.0);
+
+ status = Ok;
+ }
}
}