169 lines
4.2 KiB
C++
169 lines
4.2 KiB
C++
#include "avr.h"
|
|
#include "global.h"
|
|
#include "hardware.h"
|
|
#include "timer.h"
|
|
#include "util.h"
|
|
#include "LineSensor.h"
|
|
#include "LineSensorArray.h"
|
|
#include "Motor.h"
|
|
#include "Navigation.h"
|
|
#include "Srf10.h"
|
|
|
|
|
|
static void delay() {
|
|
for(unsigned long i = 0; i < 25000; i++)
|
|
asm("nop");
|
|
}
|
|
|
|
|
|
int main() {
|
|
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);
|
|
srf10Right->setGain(DISTANCE_GAIN);
|
|
|
|
srf10Left->setUnit(Centimeters);
|
|
srf10Left->setGain(DISTANCE_GAIN);
|
|
|
|
Srf10 *srf10Last = srf10Left;
|
|
|
|
do {
|
|
lineSensorArray->update();
|
|
} while(!lineSensorArray->calibrate());
|
|
|
|
ledOn();
|
|
waitForButton(0);
|
|
ledOff();
|
|
|
|
sleep(4000);
|
|
|
|
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!!
|
|
|
|
if(srf10Last->hasDistance() && srf10Last->getDistance() <= DISTANCE_RECOG) {
|
|
navigation->setSpeed(DEFAULT_SPEED);
|
|
navigation->setDirection(90.0);
|
|
|
|
status = Debris;
|
|
break;
|
|
}
|
|
|
|
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:
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|