From e7e1634240c78c7abbdc7fc6e0af1beddaea1d45 Mon Sep 17 00:00:00 2001 From: neoraider Date: Tue, 17 Apr 2007 22:10:05 +0000 Subject: Linienverfolgung implementiert --- Srf10.h | 6 +-- Tcs230.cpp | 28 +++++++++++++ Tcs230.h | 31 ++++++++++++++ avr.cpp | 20 +++++++++ avr.h | 3 ++ global.h | 19 +++++++++ hardware.cpp | 11 +++-- main.cpp | 133 +++++++++++++++++++++++++++++++++++++++++++++++++++-------- 8 files changed, 227 insertions(+), 24 deletions(-) create mode 100644 Tcs230.cpp create mode 100644 Tcs230.h create mode 100644 global.h diff --git a/Srf10.h b/Srf10.h index efb8cfb..c7417b8 100644 --- a/Srf10.h +++ b/Srf10.h @@ -1,5 +1,5 @@ -#ifndef SRF10_H_ -#define SRF10_H_ +#ifndef _ROBOCUP_SRF10_H_ +#define _ROBOCUP_SRF10_H_ #include @@ -41,4 +41,4 @@ public: long getDistance() {return distance;} }; -#endif /*SRF10_H_*/ +#endif diff --git a/Tcs230.cpp b/Tcs230.cpp new file mode 100644 index 0000000..796bcb3 --- /dev/null +++ b/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); +} diff --git a/Tcs230.h b/Tcs230.h new file mode 100644 index 0000000..7f25869 --- /dev/null +++ b/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 diff --git a/avr.cpp b/avr.cpp index fca50fb..cd528ba 100644 --- a/avr.cpp +++ b/avr.cpp @@ -1,5 +1,6 @@ #include "avr.h" #include "adc.h" +#include "global.h" #include "util.h" #include @@ -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; +} diff --git a/avr.h b/avr.h index c75d4cf..851f1ff 100644 --- a/avr.h +++ b/avr.h @@ -7,4 +7,7 @@ int getButton(); void waitForButton(int i); +void beep(unsigned long freq); +void beepOff(); + #endif diff --git a/global.h b/global.h new file mode 100644 index 0000000..6aa13e4 --- /dev/null +++ b/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 diff --git a/hardware.cpp b/hardware.cpp index aa3159a..51ae18e 100644 --- a/hardware.cpp +++ b/hardware.cpp @@ -19,13 +19,18 @@ void initHardware() { DDRD = 0xFF; PORTD = 0x00; - TCCR0 = 0x62; - TCCR1A = 0xA1; - TCCR1B = 0x82; + TCCR0 = 0x62; OCR0 = 0; + + TCCR1A = 0xA1; OCR1A = 0; + + TCCR1B = 0x82; OCR1B = 0; + TCCR2 = 0x18; + OCR2 = 0; + initADC(); initI2C(); } 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