From e7eeeb0d4e825a175c67cd1c0256c91362517ebb Mon Sep 17 00:00:00 2001 From: neoraider Date: Wed, 18 Apr 2007 23:00:05 +0000 Subject: Entfernungssensoren in Ordnung gebracht, Hindernisumfahrung implementiert. --- Srf10.cpp | 20 ++++++++++++---- Srf10.h | 2 ++ global.h | 10 +++++++- hardware.cpp | 16 ++++++------- i2c.cpp | 78 ++++++++++++++++++++++++++++++++---------------------------- main.cpp | 66 +++++++++++++++++++++++++++++++++++++------------- timer.cpp | 18 ++++++++++++++ timer.h | 6 +++++ 8 files changed, 150 insertions(+), 66 deletions(-) create mode 100644 timer.cpp create mode 100644 timer.h 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 +#include 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 @@ -9,41 +10,43 @@ static void I2CStop(); static bool I2CStartSend(uint8_t addr) { - TWCR = (1<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 + + +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 -- cgit v1.2.3