void InitialSearchRun() { /* INITIAL SEARCH */ while(!(getDist(maze[mouse.x][mouse.y]) == 0)) { updateWalls(); mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; if(isJunction()==1) { floodFill(maze, 'C', mouse.x, mouse.y); /* if(firstTurn == 'R') { floodFill(maze, '7', mouse.x, mouse.y); } else { floodFill(maze, '6', mouse.x, mouse.y); }*/ } searchMove(); stopAndFreezeWithButton(); } }
void VirtualWallLayer::onWallMessageCallback(const WallPtr& msg) { WallInfo wi; if(worldToMap(msg->from.x, msg->from.y, wi.x1, wi.y1) && worldToMap(msg->to.x, msg->to.y, wi.x2, wi.y2)) { walls[msg->identifier] = wi; updateWalls(); } else { walls.erase(msg->identifier); } }
void ReturnSearchRun() { turnOnLeds(0); floodFill(maze, firstTurn, mouse.x, mouse.y); while(!(getDist(maze[mouse.x][mouse.y]) == 0)) { updateWalls(); mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; floodFill(maze, firstTurn, mouse.x, mouse.y); searchMove(); stopAndFreezeWithButton(); } }
void solveMaze() { //Reset maze to 0 initializeMaze(&maze); //We Know at the starting point, there's something behind us setN(&maze[0][0], 1); mouse.direction.x = 0; mouse.direction.y = -1; mouse.x = 0; mouse.y = 0; //Disable the mouse for now enableDrive(0); turnOnTimers(0, 0); int areWeSearching = UserInterfaceIntro(); //Does the user want to skip the search phase and load a maze from EEPROM if(areWeSearching) { /* SEARCH MAZE */ for(int i = 0; i < 10; i++) { turnOnLeds(7); _delay_ms(10); turnOnLeds(0); _delay_ms(90); } //Init Mouse enableDrive(1); turnOnTimers(1, 1); setDirection(0, 0); //Update Sensors updateSensors(); updateSensors(); updateWalls(); //Go Forward first block mouse.IR_LONG_CHECK_LEFT = 2; mouse.IR_LONG_CHECK_RIGHT = 2; straight(480, 0, mouse.maxVelocity, mouse.maxVelocity, mouse.acceleration, mouse.deceleration); mouse.x += mouse.direction.x; mouse.y -= mouse.direction.y; /* SEARCH */ InitialSearchRun(); //Search is Complete! updateWalls(); /* TURN AROUND */ mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; StopFromSpeedHalf(); mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; moveBackwards(); //Save Maze to EEPROM saveCurrentMaze(); writeMemByte(MOUSE_DIRECTION, firstTurn); mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; moveForwardHalf(); //Current Mouse Direction int dirx = mouse.direction.x; int diry = mouse.direction.y; //Reverse and go back mouse.direction.x = -dirx; mouse.direction.y = -diry; //Set Position to next block mouse.x += mouse.direction.x; mouse.y -= mouse.direction.y; /* RETURN SEARCH*/ ReturnSearchRun(); //Turn Around mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; StopFromSpeedHalf(); mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; moveBackwards(); //Save Maze to EEPROM saveCurrentMaze(); writeMemByte(MOUSE_DIRECTION, firstTurn); /* PICK UP AND PLACE MOUSE */ enableDrive(0); waitForButtonPressed(); } /* FAST RUN */ FastRun(); //Completed Search run, go back and search some more turnAroundInPlace(); floodFill(maze, firstTurn, mouse.x, mouse.y); /* RETURN */ ReturnSearchRun(); //Turn Around mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; StopFromSpeedHalf(); mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0; moveBackwards(); //Save Maze to EEPROM saveCurrentMaze(); writeMemByte(MOUSE_DIRECTION, firstTurn); stopMouse(); while(!isButtonPushed(1)); printMaze(maze); }