summaryrefslogtreecommitdiffstats
path: root/main.cpp
diff options
context:
space:
mode:
authorneoraider <devnull@localhost>2007-04-18 00:10:05 +0200
committerneoraider <devnull@localhost>2007-04-18 00:10:05 +0200
commite7e1634240c78c7abbdc7fc6e0af1beddaea1d45 (patch)
treee09406ef8e02fbd7923734acb2e36d5e93d8f6db /main.cpp
parentd02fbc84105ff2da74f03fd658ace8919e3e9437 (diff)
downloadrc2007-rescue-e7e1634240c78c7abbdc7fc6e0af1beddaea1d45.tar
rc2007-rescue-e7e1634240c78c7abbdc7fc6e0af1beddaea1d45.zip
Linienverfolgung implementiert
Diffstat (limited to 'main.cpp')
-rw-r--r--main.cpp133
1 files changed, 115 insertions, 18 deletions
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;
}