From e7e1634240c78c7abbdc7fc6e0af1beddaea1d45 Mon Sep 17 00:00:00 2001
From: neoraider <devnull@localhost>
Date: Tue, 17 Apr 2007 22:10:05 +0000
Subject: Linienverfolgung implementiert

---
 main.cpp | 133 ++++++++++++++++++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 115 insertions(+), 18 deletions(-)

(limited to 'main.cpp')

diff --git a/main.cpp b/main.cpp
index 9fddb57..0da17a9 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,32 +1,129 @@
-#include "hardware.h"
 #include "avr.h"
+#include "global.h"
+#include "hardware.h"
+#include "util.h"
+#include "LineSensor.h"
+#include "LineSensorArray.h"
 #include "Motor.h"
 #include "Navigation.h"
+#include "Srf10.h"
+
+
+
+
+
+
 
 
-Motor *motorLeft = new Motor(&PORTD, &OCR1BL, 0x01, 0x02);
-Motor *motorBack = new Motor(&PORTD, &OCR1AL, 0x04, 0x08);
-Motor *motorRight = new Motor(&PORTB, &OCR0, 0x01, 0x02);
 
-Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motorRight, 300.0);
 
 
 static void delay() {
-  for(unsigned long i = 0; i < 25000; i++)
-  	asm("nop");
+	for(unsigned long i = 0; i < 25000; i++)
+		asm("nop");
 }
 
 
 int main() {
-  initHardware();
-  
-  navigation->setSpeed(1.0);
-  
-  for(int i = 0; ; i = (i+1)%360) {
-    navigation->setDirection(i);
-    
-    delay();
-  }
-  
-  return 0;
+	Status status = Ok;
+	
+	initHardware();
+	
+	Motor *motorLeft = new Motor(&PORTD, &OCR1BL, 0x01, 0x02);
+	Motor *motorBack = new Motor(&PORTD, &OCR1AL, 0x04, 0x08);
+	Motor *motorRight = new Motor(&PORTB, &OCR0, 0x01, 0x02);
+	
+	Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motorRight, 300.0);
+	
+	LineSensor *lineSensors[5];
+	
+	for(int i = 0; i < 5; i++)
+		lineSensors[i] = new LineSensor(i);
+	
+	LineSensorArray *lineSensorArray = new LineSensorArray(lineSensors);
+	
+	lineSensorArray->setMinimumDifference(CAL_MIN_DIFF);
+	lineSensorArray->setMaximumWhiteDifference(CAL_MAX_WHITE_DIFF);
+	
+	//Srf10 *srf10Right = new Srf10(0xE0);
+	Srf10 *srf10Left = new Srf10(0xE2);
+	
+	//srf10Right->setUnit(Centimeters);
+	srf10Left->setUnit(Centimeters);
+	
+	//Srf10 *srf10Last = srf10Left;
+	
+	
+	do {
+		lineSensorArray->update();
+	} while(!lineSensorArray->calibrate());
+	
+	navigation->setSpeed(DEFAULT_SPEED);
+	
+	while(true) {
+		lineSensorArray->update();
+		
+		//srf10Last = (srf10Last==srf10Left)?srf10Right:srf10Left;
+		//srf10Last->updateDistance();
+		
+		switch(status) {
+			case Ok:
+				if(lineSensorArray->getLine() == 0.0 && lineSensorArray->getSharpness() == 1.0)
+					lineSensorArray->calibrate();
+				
+				// TODO: victims!!
+				
+				// TODO: debris!!
+				
+				if(lineSensorArray->getSharpness() < 0.0)
+					break;
+				else if(lineSensorArray->getSharpness() == 0.0) {
+					// TODO: white?
+				}
+				else {
+					if(lineSensorArray->getLine() == 0.0) {
+						navigation->setSpeed(DEFAULT_SPEED);
+						navigation->setDirection(0.0);
+						navigation->setSpin(0.0);
+					}
+					else if(ABS(lineSensorArray->getLine()) < 1.0) {
+						navigation->setSpeed(DEFAULT_SPEED);
+						navigation->setDirection(0.0);
+						
+						if(lineSensorArray->getLine() < 0.0)
+							navigation->setSpin(CURVE_SPIN);
+						else
+							navigation->setSpin(-CURVE_SPIN);
+					}
+					else if(ABS(lineSensorArray->getLine()) < 1.5) {
+						navigation->setSpeed(DEFAULT_SPEED);
+						
+						if(lineSensorArray->getLine() < 0.0) {
+							navigation->setDirection(STRAFE_DIRECTION);
+							navigation->setSpin(CURVE_SPIN);
+						}
+						else {
+							navigation->setDirection(-STRAFE_DIRECTION);
+							navigation->setSpin(-CURVE_SPIN);
+						}
+					}
+					else {
+						navigation->setSpeed(0.0);
+						
+						if(lineSensorArray->getLine() < 0.0)
+							navigation->setSpin(TURN_SPIN);
+						else
+							navigation->setSpin(-TURN_SPIN);
+					}
+				}
+					
+				break;
+			case White:
+				break;
+			case Debris:
+				break;
+		}
+	}
+	
+	return 0;
 }
-- 
cgit v1.2.3