/**
* Anropar nuvarande kommando i kön
*/
void executeCommand(uint8_t command)
{
glob_curComm = command;
glob_syncSpike = 255;
switch(command)
{
case FORWARD_COMMAND:
regulateStraight();
if(!(glob_routeLength != 1 && glob_route[glob_routeLength-2] == FORWARD_COMMAND))
{
_delay_ms(500);
}
break;
case RIGHT_90_COMMAND:
turnRight90();
_delay_ms(300);
break;
case LEFT_90_COMMAND:
turnLeft90();
_delay_ms(300);
break;
case RIGHT_45_COMMAND:
turnRight45();
break;
case LEFT_45_COMMAND:
turnLeft45();
break;
case VIRTUAL_REVERSE_COMMAND:
virtualTurn();
break;
case CUSTOM_STEERING_COMMAND:
sendAllMapData();
customSteering();
break;
default:
break;
}
glob_curComm = NULL_COMMAND;
_delay_ms(500);
}
void main(void)
{
init();
STOP();
lcd_set_cursor(0x00);
lcd_write_string("(-,-) - -- --- -"); //x/y of robot, explore/return, zone of victim/got victim, walls, way went
lcd_set_cursor(0x40);
lcd_write_string("- - - (3,1) GREG"); //orientation, cliff detected, v.wall detected, x/y of final destination
char victimIndicator = 0;
while(!home)
{
if(start.pressed && ready == FALSE)
{
findWalls();
if(leftWall && rightWall && frontWall) //if facing east
turnAround(); //turn to face west
else if (rightWall && frontWall) //if facing north
turnLeft90(); //turn to face west
else if(leftWall && frontWall) //if facing south
turnRight90(); //turn to face west
ready = TRUE; //commence exploring the map
lcd_set_cursor(0x06);
lcd_write_data('E'); //Explore mode
play_iCreate_song(1);
}
//EEPROM Serial Transfer
if(eepromSerial.pressed && ready == FALSE)
{
eepromSerial.pressed = FALSE;
lcd_set_cursor(0x00);
lcd_write_string("EEPROM Serial ");
lcd_set_cursor(0x40);
lcd_write_string("Please Wait... ");
writeEEPROMTestData();
EEPROMToSerial();
lcd_set_cursor(0x40);
lcd_write_string("Complete ");
}
if(start.pressed)
{
ready = TRUE;
checkForFinalDestination();
lookForVictim();
findWalls();
play_iCreate_song(5);
if(leftWall)
{
rotateIR(24,CCW);
leftAngleCorrect();
rotateIR(24,CW);
}
play_iCreate_song(5);
if(frontWall)
frontWallCorrect();
play_iCreate_song(5);
switch(node) // Gives the Create specific movement instructions for certain squares to go shortest path home and collecting the victim
{
case 0:
goToNextCell();
break;
case 1:
if (goingHome)
{
if (victimZone == 1)
goRight();
else if (getOrientation() == EAST)
goForward();
else if (getOrientation() == SOUTH)
goRight();
else
goToNextCell();
}
else
goToNextCell();
break;
case 2:
if (goingHome)
{
if (victimZone == 2)
goForward();
else if (getOrientation() == SOUTH)
goRight();
else if (getOrientation() == NORTH)
goLeft();
else
goToNextCell();
}
else
goToNextCell();
break;
case 3:
if (goingHome)
{
if (victimZone == 3)
goRight();
else if (getOrientation() == EAST)
goForward();
else if (getOrientation() == SOUTH)
goLeft();
else
goToNextCell();
}
else
goToNextCell();
break;
case 4:
if (getOrientation() == EAST)
goRight();
else
goToNextCell();
break;
case 5:
if (getOrientation() == NORTH)
goRight();
else
goToNextCell();
break;
case 6:
if (getOrientation() == WEST)
{
play_iCreate_song(6);
goForward();
}
else
goToNextCell();
break;
default:
break;
}
play_iCreate_song(5);
if(getSuccessfulDrive())
{
//Send EEPROM data for current cell
if(xVictim == xCoord && yVictim == yCoord)
{
victimIndicator = 1;
}
/*if(xVirtual == xCoord && yVirtual == yCoord)
{
switch(dVirtual)
{
case WEST:
{
updateMapData(1,0,0,0,victimIndicator,getOrientation());
break;
}
case SOUTH:
{
updateMapData(0,1,0,0,victimIndicator,getOrientation());
break;
}
case EAST:
{
updateMapData(0,0,1,0,victimIndicator,getOrientation());
break;
}
case NORTH:
{
updateMapData(0,0,0,1,victimIndicator,getOrientation());
break;
}
default:
updateMapData(0,0,0,0,victimIndicator,getOrientation());
}
}*/
updateMapData(0,0,0,0,victimIndicator,getOrientation());
victimIndicator = 0;
updateLocation();
updateNode();
if(goingHome)
checkIfHome();
}
}
}
}
int locateCandle(int speed, bool turnRight) {
int found = 0;
int distance = TURN90;
int maxVal = 0;
int angleAtMaxVal;
if (turnRight) {
setMotorSpeed(speed,speed, FORWARD, BACKWARD);
}
else{
setMotorSpeed(speed,speed, BACKWARD, FORWARD);
}
int prevVal = checkOdometry();
while (distance > 0){
if(turnRight) {
int val = max(analogRead(UV_LEFT_PIN),analogRead(UV_RIGHT_PIN));
if(val > maxVal){
maxVal = val;
angleAtMaxVal = distance;
}
}
else {
int val = max(analogRead(UV_LEFT_PIN),analogRead(UV_RIGHT_PIN));
if(val > maxVal){
maxVal = val;
angleAtMaxVal = distance;
}
}
int val = checkOdometry();
if (prevVal != val){
distance--;
prevVal = val;
}
}
if (maxVal < UV_THRESH) {
if (turnRight) turnRight90();
else turnLeft90();
return NO_CANDLE;
}
stop();
//reverse direction
if (turnRight) {
turnLeftUntil(angleAtMaxVal);
}
else{
turnRightUntil(angleAtMaxVal);
}
// put candle in middle of sensors
while (true) {
if(turnRight) {
Serial.print("turnRight ");Serial.print(analogRead(UV_LEFT_PIN));Serial.print(" ");Serial.println(analogRead(UV_RIGHT_PIN));
if (analogRead(UV_RIGHT_PIN)>= analogRead(UV_LEFT_PIN)) {
break;
}
}
else {
Serial.print("turnLeft ");Serial.print(analogRead(UV_LEFT_PIN));Serial.print(" ");Serial.println(analogRead(UV_RIGHT_PIN));
if (analogRead(UV_LEFT_PIN)>= analogRead(UV_RIGHT_PIN)) {
break;
}
}
}
stop();
if (angleAtMaxVal <= .1 * TURN90) return TO_SIDE;
else if (angleAtMaxVal <= .8 * TURN90) return KIDDIE_CORNER;
else return STRAIGHT_AHEAD;
}
