Entfernungssensoren in Ordnung gebracht, Hindernisumfahrung implementiert.

8 files changed, 150 insertions, 66 deletions

diff --git a/Srf10.cpp b/Srf10.cpp
index 329fc8a..a66828b 100644
--- a/Srf10.cpp
+++ b/Srf10.cpp
@@ -1,6 +1,7 @@
 #include "Srf10.h"
 
 #include "i2c.h"
+#include "timer.h"
 
 
 Srf10::Srf10(uint8_t id)
@@ -13,6 +14,7 @@ Srf10::Srf10(uint8_t id)
 	unit = Inches;
 	
 	distance = 0;
+	has_distance = false;
 	
 	firmware = readFirmware();
 }
@@ -46,15 +48,25 @@ bool Srf10::setRange(uint8_t range) {
 
 long Srf10::updateDistance() {
 	uint8_t data[2] = {0, unit};
+	uint16_t d;
+	
 	
 	if(!I2CSend(id, data, 2)) return -1;
 	
-	while(readFirmware() != 0xFF);
+	do {
+		sleep(1);
+	} while(readFirmware() == 0xFF);
 	
 	if(!I2CSendByte(id, 2)) return -1;
-	if(!I2CRecv(id, data, 2)) return -1;
+	if(I2CRecv(id, data, 2) < 2) return -1;
+	
+	d = (((uint16_t)data[0]) << 8) | data[1];
 	
-	distance = (((uint16_t)data[0]) << 8) | data[1];
+	if(d == 0) has_distance = false;
+	else {
+		has_distance = true;
+		distance = d;
+	}
 	
-	return distance;
+	return d;
 }
diff --git a/Srf10.h b/Srf10.h
index c7417b8..1b69018 100644
--- a/Srf10.h
+++ b/Srf10.h
@@ -19,6 +19,7 @@ private:
 	int firmware;
 	
 	long distance;
+	bool has_distance;
 	
 	Srf10Units unit;
 	
@@ -39,6 +40,7 @@ public:
 	
 	long updateDistance();
 	long getDistance() {return distance;}
+	bool hasDistance() {return has_distance;}
 };
 
 #endif
diff --git a/global.h b/global.h
index 6aa13e4..3bfb285 100644
--- a/global.h
+++ b/global.h
@@ -2,6 +2,7 @@
 #define _ROBOCUP_GLOBAL_H_
 
 #define F_CPU 16000000UL
+#define TIMER_PRESCALER 8
 
 #define DEFAULT_SPEED 0.7
 #define CURVE_SPIN 0.3
@@ -11,9 +12,16 @@
 #define CAL_MIN_DIFF 0.3
 #define CAL_MAX_WHITE_DIFF 0.1
 
+#define DISTANCE_GAIN 9
+#define DISTANCE_RECOG 4
+#define DISTANCE_MAX 6
+#define DISTANCE_MIN 5
+#define DISTANCE_SPIN 0.3
+#define DISTANCE_ANGLE 30.0
+
 
 enum Status {
-	Ok, White, Debris
+	Ok, White, Debris, DebrisWhite, Turn, TurnWhite
 };
 
 #endif
diff --git a/hardware.cpp b/hardware.cpp
index 51ae18e..e652dda 100644
--- a/hardware.cpp
+++ b/hardware.cpp
@@ -4,6 +4,7 @@
 #include "i2c.h"
 
 #include <avr/io.h>
+#include <avr/interrupt.h>
 
 
 void initHardware() {
@@ -19,18 +20,15 @@ void initHardware() {
   DDRD = 0xFF;
   PORTD = 0x00;
   
-  TCCR0 = 0x62;
-  OCR0 = 0;
-  

-  TCCR1A = 0xA1;
-  OCR1A = 0;
-  

+  TCCR0 = 0x62;

+  TCCR1A = 0xA1;

   TCCR1B = 0x82;
-  OCR1B = 0;
-  
   TCCR2 = 0x18;
-  OCR2 = 0;
+  
+  TIMSK = 0x01;
   
   initADC();
   initI2C();
+  
+  sei();
 }
diff --git a/i2c.cpp b/i2c.cpp
index 61450fa..39b6b28 100644
--- a/i2c.cpp
+++ b/i2c.cpp
@@ -1,4 +1,5 @@
 #include "i2c.h"
+#include "avr.h"
 
 #include <util/twi.h>
 
@@ -9,41 +10,43 @@ static void I2CStop();
 
 
 static bool I2CStartSend(uint8_t addr) {
-  TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA);
-  
-  while(!(TWCR & (1<<TWINT)));
-      
-  if (TW_STATUS != TW_START) return false;
-  
-  TWDR = (addr&0xFE);
-  TWCR = (1<<TWEN)|(1<<TWINT);
-  
-  while(!(TWCR & (1<<TWINT)));
-  
-  if(TW_STATUS == TW_MT_SLA_ACK)
-    return true;
-  
-  I2CStop();
-  return false;
+  while(true) {
+    TWCR |= (1<<TWINT)|(1<<TWSTA);
+    
+    while(!(TWCR & (1<<TWINT)));
+    
+    if(TW_STATUS != TW_START && TW_STATUS != TW_REP_START) continue;
+    
+    TWDR = (addr&0xFE);
+    TWCR = (1<<TWEN)|(1<<TWINT);
+    
+    while(!(TWCR & (1<<TWINT)));
+    
+    if(TW_STATUS == TW_MT_SLA_ACK)
+      return true;
+    
+    I2CStop();
+  }
 }
 
 static bool I2CStartRecv(uint8_t addr) {
-  TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA);
-  
-  while(!(TWCR & (1<<TWINT)));
-      
-  if (TW_STATUS != TW_START) return false;
-  
-  TWDR = (addr|1);
-  TWCR = (1<<TWEN)|(1<<TWINT);
-  
-  while(!(TWCR & (1<<TWINT)));
-  
-  if(TW_STATUS == TW_MR_SLA_ACK)
-    return true;
-  
-  I2CStop();
-  return false;
+  while(true) {
+    TWCR |= (1<<TWINT)|(1<<TWSTA);
+    
+    while(!(TWCR & (1<<TWINT)));
+        
+    if(TW_STATUS != TW_START && TW_STATUS != TW_REP_START) continue;
+    
+    TWDR = (addr|1);
+    TWCR |= (1<<TWINT);
+    
+    while(!(TWCR & (1<<TWINT)));
+    
+    if(TW_STATUS == TW_MR_SLA_ACK)
+      return true;
+    
+    I2CStop();
+  }
 }
 
 static void I2CStop() {
@@ -83,7 +86,7 @@ bool I2CSend(uint8_t addr, uint8_t *data, int length) {
     TWCR = (1<<TWEN)|(1<<TWINT);
     while(!(TWCR & (1<<TWINT)));
     
-    if(TW_STATUS != TW_MT_DATA_ACK) {
+    if(TW_STATUS != TW_MT_DATA_ACK && i < length-1) {
       I2CStop();
       return false;
     }
@@ -96,6 +99,8 @@ bool I2CSend(uint8_t addr, uint8_t *data, int length) {
 bool I2CRecvByte(uint8_t addr, uint8_t *data) {
   if(!I2CStartRecv(addr)) return false;
   
+  TWCR = (1<<TWEN)|(1<<TWINT);
+  
   while(!(TWCR & (1<<TWINT)));
   
   if(TW_STATUS != TW_MR_DATA_ACK && TW_STATUS != TW_MR_DATA_NACK) {
@@ -107,23 +112,24 @@ bool I2CRecvByte(uint8_t addr, uint8_t *data) {
   TWCR = (1<<TWEN)|(1<<TWINT);
   
   I2CStop();
+  
   return true;
 }
 
 int I2CRecv(uint8_t addr, uint8_t *data, int length) {
   if(!I2CStartRecv(addr)) return -1;
   
+  TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
+  
   for(int i = 0; i < length-1; i++) {
     while(!(TWCR & (1<<TWINT)));
     
     switch(TW_STATUS) {
       case TW_MR_DATA_ACK:
         data[i] = TWDR;
-        TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
         break;
       case TW_MR_DATA_NACK:
         data[i] = TWDR;
-        TWCR = (1<<TWEN)|(1<<TWINT);
         I2CStop();
         return i+1;
       default:
@@ -132,13 +138,13 @@ int I2CRecv(uint8_t addr, uint8_t *data, int length) {
     }
   }
   
+  TWCR = (1<<TWEN)|(1<<TWINT);
   while(!(TWCR & (1<<TWINT)));
   
   switch(TW_STATUS) {
     case TW_MR_DATA_ACK:
     case TW_MR_DATA_NACK:
       data[length-1] = TWDR;
-      TWCR = (1<<TWEN)|(1<<TWINT);
       I2CStop();
       return length;
   }
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;
+				}
 		}
 	}
 	
diff --git a/timer.cpp b/timer.cpp
new file mode 100644
index 0000000..b4a1b12
--- /dev/null
+++ b/timer.cpp
@@ -0,0 +1,18 @@
+#include "timer.h"
+#include "global.h"
+
+#include <avr/interrupt.h>
+
+
+static volatile unsigned long ticks = 0;
+
+
+SIGNAL(SIG_OVERFLOW0) {
+	ticks++;	
+}
+
+void sleep(unsigned long ms) {
+	unsigned long t = ticks + ms * (F_CPU/TIMER_PRESCALER/256000);
+	
+	while(ticks < t);
+}
diff --git a/timer.h b/timer.h
new file mode 100644
index 0000000..ad4bf05
--- /dev/null
+++ b/timer.h
@@ -0,0 +1,6 @@
+#ifndef _ROBOCUP_TIMER_H_
+#define _ROBOCUP_TIMER_H_
+
+void sleep(unsigned long ms);
+
+#endif