/** * 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; }