Entfernungssensoren in Ordnung gebracht, Hindernisumfahrung implementiert.

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;
+				}
 		}
 	}