Linienverfolgung implementiert

Srf10.h

@@ -1,5 +1,5 @@
-#ifndef SRF10_H_
+#ifndef _ROBOCUP_SRF10_H_
-#define SRF10_H_
+#define _ROBOCUP_SRF10_H_
 
 #include <stdint.h>
 
@@ -41,4 +41,4 @@ public:
 	long getDistance() {return distance;}
 };
 
-#endif /*SRF10_H_*/
+#endif

Tcs230.cpp

@@ -0,0 +1,28 @@
+#include "Tcs230.h"
+#include "avr.h"
+
+
+Tcs230::Tcs230()
+{
+	enabled = false;
+	color = Red;
+	scaling = Off;
+}
+
+void Tcs230::setEnabled(bool enabled) {
+	this->enabled = enabled;
+	
+	PORTC = (PORTC&~0x40)|(enabled?0x40:0);
+}
+
+void Tcs230::setColor(Tcs230Colors color) {
+	this->color = color;
+	
+	PORTC = (PORTC&~0x0C)|((color<<2)&0x0C);
+}
+
+void Tcs230::setScaling(Tcs230Scalings scaling) {
+	this->scaling = scaling;
+	
+	PORTC = (PORTC&~0x30)|((scaling<<4)&0x30);
+}

Tcs230.h

@@ -0,0 +1,31 @@
+#ifndef _ROBOCUP_TCS230_H_
+#define _ROBOCUP_TCS230_H_
+
+enum Tcs230Colors {
+	Red = 0, Blue = 1, Clear = 2, Green = 3
+};
+
+enum Tcs230Scalings {
+	Off = 0, TwoPercent = 1, TwentyPercent = 2, Full = 3
+};
+
+class Tcs230
+{
+private:
+	bool enabled;
+	Tcs230Colors color;
+	Tcs230Scalings scaling;
+public:
+	Tcs230();
+	
+	bool getEnabled() {return enabled;}
+	void setEnabled(bool enabled);
+	
+	Tcs230Colors getColor() {return color;}
+	void setColor(Tcs230Colors color);
+	
+	Tcs230Scalings getScaling() {return scaling;}
+	void setScaling(Tcs230Scalings scaling);
+};
+
+#endif

avr.cpp

@@ -1,5 +1,6 @@
 #include "avr.h"
 #include "adc.h"
+#include "global.h"
 #include "util.h"
 
 #include <stdint.h>
@@ -21,3 +22,22 @@ void waitForButton(int i) {
   while(getButton() != CLAMP(0, i, 5));
   while(getButton() != 0);
 }
+
+void beep(unsigned long freq) {
+	const int prescalers[7] = {1, 8, 32, 64, 128, 256, 1024};
+	
+	for(int i = 0; i < 7; i++) {
+		if(F_CPU/freq/2/prescalers[i] <= 256) {
+			TCCR2 = (TCCR2&~0x07)|(i+1);
+			OCR2 = F_CPU/freq/2/prescalers[i] - 1;
+			
+			return;
+		}
+	}
+	
+	beepOff();
+}
+
+void beepOff() {
+	TCCR2 &= ~0x07;
+}

avr.h

@@ -7,4 +7,7 @@
 int getButton();
 void waitForButton(int i);
 
+void beep(unsigned long freq);
+void beepOff();
+
 #endif

global.h

@@ -0,0 +1,19 @@
+#ifndef _ROBOCUP_GLOBAL_H_
+#define _ROBOCUP_GLOBAL_H_
+
+#define F_CPU 16000000UL
+
+#define DEFAULT_SPEED 0.7
+#define CURVE_SPIN 0.3
+#define TURN_SPIN 0.5
+#define STRAFE_DIRECTION 30.0
+
+#define CAL_MIN_DIFF 0.3
+#define CAL_MAX_WHITE_DIFF 0.1
+
+
+enum Status {
+	Ok, White, Debris
+};
+
+#endif

hardware.cpp

@@ -20,12 +20,17 @@ void initHardware() {
   PORTD = 0x00;
   
   TCCR0 = 0x62;
-  TCCR1A = 0xA1;
-  TCCR1B = 0x82;
   OCR0 = 0;
+  
+  TCCR1A = 0xA1;
   OCR1A = 0;
+  
+  TCCR1B = 0x82;
   OCR1B = 0;
   
+  TCCR2 = 0x18;
+  OCR2 = 0;
+  
   initADC();
   initI2C();
 }

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++)
+	for(unsigned long i = 0; i < 25000; i++)
-  	asm("nop");
+		asm("nop");
 }
 
 
 int main() {
-  initHardware();
+	Status status = Ok;
 	
-  navigation->setSpeed(1.0);
+	initHardware();
 	
-  for(int i = 0; ; i = (i+1)%360) {
+	Motor *motorLeft = new Motor(&PORTD, &OCR1BL, 0x01, 0x02);
-    navigation->setDirection(i);
+	Motor *motorBack = new Motor(&PORTD, &OCR1AL, 0x04, 0x08);
+	Motor *motorRight = new Motor(&PORTB, &OCR0, 0x01, 0x02);
 	
-    delay();
+	Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motorRight, 300.0);
-  }
 	
-  return 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;
 }