Maze-Solving Robot
Maze-Solving Robot
April 2022
April 2022
Embedded Files
A robot that computes the most efficient path to any given coordinates and drives there.
A robot that computes the most efficient path to any given coordinates and drives there.
Software Design
Software Design
The functions written for this program can be split into two phases; the computational (virtual) phase, and the navigational (physical) phase.
During the computational phase, the robot uses a right wall-following algorithm to determine an unoptimized feasible path to the target destination and logs its virtual movements in an array. It then runs an optimization loop to remove any extraneous movements and truncate the array to only hold the least required movements. The virtual maze is constructed using 2D arrays of structs, where dot operators are called on members of the struct representing the east, west, north, and south walls of each cell. Please note that although the virtual maze is modeled as a wall maze, the physical maze (shown in the video) is a line maze (i.e. the line runs through the middle of each cell of the wall maze).
During the navigational phase, the robot acts on the newly-optimized path and advances cell-by-cell, determining whether to turn left, right, or go straight depending on its next movement (array[i]) and the current (array[i-1]). Simultaneously, the robot's location and heading are displayed on the screen in a mock GPS fashion.
The robot's positional information is tracked solely by motor encoder values. Sufficient time was spent tuning encoder values for turning 90° and moving forward by one cell to achieve the desired accuracy.
Please see the collapsed section below for the full source code.
Mechanical Design
Mechanical Design
Since this project was primarily an exercise in algorithm programming, there was only one implementation of mechanical design: the largest-diameter wheels were chosen to mitigate any bumps or snags that the wheels may experience when rolling over dust or other small debris on the floor.
Recommendations
Recommendations
Although fine-tuning the motor encoder values was satisfactory for this project, in the future, the color sensor should be used to track the line as it moves forward. If the color sensor is mounted on a swivelling or sliding mechanism (e.g. rack and pinion), it could also be used for detecting corners and intersections.
Acknowledgements
Acknowledgements
This project was completed with two other lab partners and contains contributions to the source code by the class instructor.
Video footage showing the robot navigating from a given starting position (0, 0) to a given target position (6,8). The spinning blue spike was for extra flair.
Note: the version of the robot shown in this video doesn't have the real-time "GPS" tracking functionality that was present in the final version.
The robot was based on the standard LEGO Mindstorms EV3 build, with some minor modifications to suit this project.
Source Code
Source Code
#pragma config(Sensor, S1, touchSensor, sensorEV3_Touch)
#pragma config(Sensor, S2, gyroSensor, sensorEV3_Gyro, modeEV3Gyro_RateAndAngle)
#pragma config(Sensor, S3, colorSensor1, sensorEV3_Color, modeEV3Color_Color)
#pragma config(Sensor, S4, colorSensor2, sensorEV3_Color, modeEV3Color_Color)
#pragma config(Motor, motorA, armMotor, tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor, motorB, leftMotor, tmotorEV3_Large, PIDControl, driveLeft, encoder)
#pragma config(Motor, motorC, rightMotor, tmotorEV3_Large, PIDControl, driveRight, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//Initialize preset values for screen and maze dimensions
const int screenHeight = 127;
const int screenWidth = 102;
const int numOfRows = 10; //rows and cols start from 0
const int numOfCols = 8;
int startHeading = 0;
int robHeading = startHeading; // 0=North, 1=East, 2=South, 3=West
//define new structure to store maze wall information
typedef struct{
int nWall; // 0 = door, 1 = wall
int eWall;
int sWall;
int wWall;
}cell;
//declare 2D arrays of structs
cell maze[numOfRows][numOfCols];
//initialize variables pertinent to path optimization and navigation
unsigned char array[200];
bool physicalNavigation = false;
unsigned char stepCounter=0;
int startRow = 8; //rows and cols start from 0
int startcol = 1;
int robRow = startRow;
int robCol = startcol;
int targetRow = 6;
int targetCol = 0;
//Computational portion
void buildOuterBorders();
void gridDraw();
void drawBot();
void turnRight();
void turnLeft();
void goFwd();
void refreshScreen();
void MazeSim();
void mazeSolverRHF();
void optimizeRoute();
//Navigational portion
void navigateOptimizedRoute();
void moveFwd();
void rotateLeft();
void rotateRight();
void rotate180();
void spinSpikyThing();
void stopAllMotors();
task main(){
//pre-fill array with 99s to differentiate between
//compass headings (e.g. 0,1,2,3) and unused indices (e.g. 99)
for(int i=0; i<200; i++){
array[i]= 99;
}
buildOuterBorders();
MazeSim();
mazeSolverRHF();
optimizeRoute();
navigateOptimizedRoute();
sleep(1000);
}
//Right wall follower algorithm to determine unoptimized path
void mazeSolverRHF(){
turnRight();
while((robCol != targetCol) || (robRow != targetRow) ){
//switch statement to determine whether to turn left or right when encountering a wall
switch(robHeading){
case 0:
if(maze[robRow][robCol].nWall == 1){
turnLeft();
continue;
}
break;
case 1: // facing east
if(maze[robRow][robCol].eWall == 1){
turnLeft();
continue;
}
break;
case 2:
if(maze[robRow][robCol].sWall == 1){
turnLeft();
continue;
}
break;
case 3:
if(maze[robRow][robCol].wWall == 1){
turnLeft();
continue;
}
break;
default: break;
}
goFwd();
turnRight();
}
}
//Simulate the maze walls using 2D array of structs
void MazeSim(){
//Vertical walls
maze[0][0].eWall = maze[0][1].wWall = 1;
maze[0][1].eWall = maze[0][2].wWall = 1;
maze[0][2].eWall = maze[0][3].wWall = 1;
maze[0][3].eWall = maze[0][4].wWall = 1;
maze[0][4].eWall = maze[0][5].wWall = 1;
maze[0][5].eWall = maze[0][6].wWall = 1;
maze[0][6].eWall = maze[0][7].wWall = 1;
maze[1][1].eWall = maze[1][2].wWall = 1;
maze[1][2].eWall = maze[1][3].wWall = 1;
maze[1][4].eWall = maze[1][5].wWall = 1;
maze[1][5].eWall = maze[1][6].wWall = 1;
maze[3][1].eWall = maze[3][2].wWall = 1;
maze[3][2].eWall = maze[3][3].wWall = 1;
maze[3][4].eWall = maze[3][5].wWall = 1;
maze[3][5].eWall = maze[3][6].wWall = 1;
maze[3][6].eWall = maze[3][7].wWall = 1;
maze[4][0].eWall = maze[4][1].wWall = 1;
maze[4][3].eWall = maze[4][4].wWall = 1;
maze[4][4].eWall = maze[4][5].wWall = 1;
maze[4][5].eWall = maze[4][6].wWall = 1;
maze[5][1].eWall = maze[5][2].wWall = 1;
maze[5][2].eWall = maze[5][3].wWall = 1;
maze[5][4].eWall = maze[5][5].wWall = 1;
maze[5][5].eWall = maze[5][6].wWall = 1;
maze[5][6].eWall = maze[5][7].wWall = 1;
maze[6][4].eWall = maze[6][5].wWall = 1;
maze[6][5].eWall = maze[6][6].wWall = 1;
maze[7][6].eWall = maze[7][7].wWall = 1;
maze[8][0].eWall = maze[8][1].wWall = 1;
maze[8][1].eWall = maze[8][2].wWall = 1;
maze[8][2].eWall = maze[8][3].wWall = 1;
maze[8][4].eWall = maze[8][5].wWall = 1;
maze[8][6].eWall = maze[8][7].wWall = 1;
maze[9][0].eWall = maze[9][1].wWall = 1;
maze[9][3].eWall = maze[9][4].wWall = 1;
maze[9][4].eWall = maze[9][5].wWall = 1;
maze[9][5].eWall = maze[9][6].wWall = 1;
maze[9][6].eWall = maze[9][7].wWall = 1;
//Horizontal walls
maze[0][1].nWall = maze[1][1].sWall = 1;
maze[0][3].nWall = maze[1][3].sWall = 1;
maze[0][6].nWall = maze[1][6].sWall = 1;
maze[1][0].nWall = maze[2][0].sWall = 1;
maze[1][4].nWall = maze[2][4].sWall = 1;
maze[1][7].nWall = maze[2][7].sWall = 1;
maze[2][0].nWall = maze[3][0].sWall = 1;
maze[2][1].nWall = maze[3][1].sWall = 1;
maze[2][4].nWall = maze[3][4].sWall = 1;
maze[2][7].nWall = maze[3][7].sWall = 1;
maze[3][1].nWall = maze[4][1].sWall = 1;
maze[3][3].nWall = maze[4][3].sWall = 1;
maze[3][7].nWall = maze[4][7].sWall = 1;
maze[4][0].nWall = maze[5][0].sWall = 1;
maze[4][3].nWall = maze[5][3].sWall = 1;
maze[4][6].nWall = maze[5][6].sWall = 1;
maze[5][0].nWall = maze[6][0].sWall = 1;
maze[5][1].nWall = maze[6][1].sWall = 1;
maze[5][2].nWall = maze[6][2].sWall = 1;
maze[5][3].nWall = maze[6][3].sWall = 1;
maze[5][6].nWall = maze[6][6].sWall = 1;
maze[5][7].nWall = maze[6][7].sWall = 1;
maze[6][0].nWall = maze[7][0].sWall = 1;
maze[6][1].nWall = maze[7][1].sWall = 1;
maze[6][2].nWall = maze[7][2].sWall = 1;
maze[6][3].nWall = maze[7][3].sWall = 1;
maze[6][4].nWall = maze[7][4].sWall = 1;
maze[7][0].nWall = maze[8][0].sWall = 1;
maze[7][1].nWall = maze[8][1].sWall = 1;
maze[7][3].nWall = maze[8][3].sWall = 1;
maze[7][4].nWall = maze[8][4].sWall = 1;
maze[7][6].nWall = maze[8][6].sWall = 1;
maze[8][4].nWall = maze[9][4].sWall = 1;
maze[8][6].nWall = maze[9][6].sWall = 1;
}
//Update orientation and position of the robot in the simulated maze
void refreshScreen(){
eraseDisplay();
gridDraw();
drawBot();
}
//Advance virtual robot one cell forward in the direction it's facing
void goFwd(){
refreshScreen();
wait1Msec(50); // waste some time to simulate motion
if (robHeading==0) { // Going Fwd North
robRow++;
}
else if (robHeading==1) { // Going Fwd East
robCol++;
}
else if (robHeading==2) { // Going Fwd South
robRow--;
}
else if (robHeading==3) { // Going Fwd West
robCol--;
}
else {
robRow = 99;
robCol = 99;
}
//Only log robot headings during simulation
//to prevent overwriting the array during physical navigation
if(!physicalNavigation){
// Log direction
array[stepCounter]=robHeading;
stepCounter++;
}
}
//Turn virtual robot to the right by updating its heading value
void turnRight(){
refreshScreen();
if (robHeading < 3){
robHeading++;
}
else robHeading=0;
}
//Turn virtual robot to the left by updating its heading value
void turnLeft(){
refreshScreen();
if (robHeading > 0){
robHeading--;
}
else robHeading=3;
}
//Simulate outer borders of the maze using preset dimensions
void buildOuterBorders(){
for(int col=0; col<numOfCols; col++){
maze[0][col].sWall = 1;
maze[numOfRows-1][col].nWall = 1;
}
for(int row=0; row<numOfRows; row++){
maze[row][0].wWall = 1;
maze[row][numOfCols-1].eWall = 1;
}
}
//Draw the interior walls of the maze based on the information
//from the MazeSim function
void gridDraw(){
int XStart=0;
int YStart=0;
int XEnd =0;
int YEnd =0;
for(int i=0;i<numOfRows;i++){
for(int j=0;j<numOfCols;j++){
if(maze[i][j].nWall==1){
XStart= j *screenWidth/numOfCols;
YStart=(i+1)*screenHeight/numOfRows;
XEnd =(j+1)*screenWidth/numOfCols;
YEnd =(i+1)*screenHeight/numOfRows;
drawLine(XStart,YStart,XEnd,YEnd);
}
if (maze[i][j].eWall==1){
XStart=(j+1)*screenWidth/numOfCols;
YStart=(i)*screenHeight/numOfRows;
XEnd =(j+1)*screenWidth/numOfCols;
YEnd =(i+1)*screenHeight/numOfRows;
drawLine(XStart,YStart,XEnd,YEnd);
}
if (maze[i][j].wWall==1){
XStart= j *screenWidth/numOfCols;
YStart=(i)*screenHeight/numOfRows;
XEnd =(j)*screenWidth/numOfCols;
YEnd =(i+1)*screenHeight/numOfRows;
drawLine(XStart,YStart,XEnd,YEnd);
}
if(maze[i][j].sWall==1){
XStart= j *screenWidth/numOfCols;
YStart=(i)*screenHeight/numOfRows;
XEnd =(j+1)*screenWidth/numOfCols;
YEnd =(i)*screenHeight/numOfRows;
drawLine(XStart,YStart,XEnd,YEnd);
}
}
}
}
//Represent the virtual robot as a symbol on the brick's screen
void drawBot(){
int RobotXpixelPos=0;
int RobotYpixelPos=0;
//To fix edge case issue of drawing the robot out of bounds
if(robCol==0){
RobotXpixelPos=screenWidth/16;
}
else{
RobotXpixelPos=(2*robCol+1)*screenWidth/16;
}
if(robRow==0){
RobotYpixelPos=screenHeight/20;
}
//If not an edge case, then execute the following code to draw the robot
else{
RobotYpixelPos=(2*robRow+1)*screenHeight/20;
}
//The drawn symbol will differ according to what direction its facing
switch(robHeading){
case 0: displayStringAt(RobotXpixelPos,RobotYpixelPos,"^"); break; // Facing North
case 1: displayStringAt(RobotXpixelPos,RobotYpixelPos,">"); break; // Facing East
case 2: displayStringAt(RobotXpixelPos,RobotYpixelPos,"V"); break; // Facing South
case 3: displayStringAt(RobotXpixelPos,RobotYpixelPos,"<"); break; // Facing West
default: break;
}
}
//Find differences of 2 between consecutive array elements and replace them with 99.
//Shift all elements up by 2 indices and check for differences again.
//Jump to the top of the array and execute the loop until the the shortest route is stored in the array.
void optimizeRoute(){
int i=0;
for(i=0; i< stepCounter-1; i++){
if (abs(array[i+1] - array[i]) == 2){
array[i+1] = array[i]= 99; // found dead end
for(int j=i; j<stepCounter; j++){ // move the two vals to end of array
array[j] = array[j+2];
}
i=0; // start over from the beginning
}
}
}
//Physically navigate the optimized route
void navigateOptimizedRoute(){
//set physicalNavigation toggle to true so goFwd() doesn't overwrite the array
physicalNavigation = true;
eraseDisplay();
//reset robot orientation and heading to beginning values
robRow = startRow;
robCol = startcol;
robHeading = startHeading;
//move forward one cell to complete the first step (robot is placed in the right heading physically)
moveFwd();
drawBot(); //drawBot() is called after each moveFwd to update the GPS
//Execute decision-making for turns whilst until the computed optimized number of steps has been reached
for(int i = 1; i < stepCounter; i++){
if(array[i]==99){
break; //break out of this loop if a 99 (unused index) has been reached
}
if(array[i-1] == 0 && array[i] == 1){
rotateRight();
robHeading = 1;
moveFwd();
drawBot();
}
else if(array[i-1] == 1 && array[i] == 2){
rotateRight();
robHeading = 2;
moveFwd();
drawBot();
}
else if(array[i-1] == 2 && array[i] == 3){
rotateRight();
robHeading = 3;
moveFwd();
drawBot();
}
else if(array[i-1] == 3 && array[i] == 0){
rotateRight();
robHeading = 0;
moveFwd();
drawBot();
}
if(array[i-1] == 0 && array[i] == 0){
moveFwd();
robHeading = 0;
drawBot();
}
else if(array[i-1] == 1 && array[i] == 1){
moveFwd();
robHeading = 1;
drawBot();
}
else if(array[i-1] == 2 && array[i] == 2){
moveFwd();
robHeading = 2;
drawBot();
}
else if(array[i-1] == 3 && array[i] == 3){
moveFwd();
robHeading = 3;
drawBot();
}
if(array[i-1] == 0 && array[i] == 3){
rotateLeft();
robHeading = 3;
moveFwd();
drawBot();
}
else if(array[i-1] == 1 && array[i] == 0){
rotateLeft();
robHeading = 0;
moveFwd();
drawBot();
}
else if(array[i-1] == 2 && array[i] == 1){
rotateLeft();
robHeading = 1;
moveFwd();
drawBot();
}
else if(array[i-1] == 3 && array[i] == 2){
rotateLeft();
robHeading = 2;
moveFwd();
drawBot();
}
if(array[i-1] == 0 && array[i] == 2){
rotate180();
robHeading = 2;
moveFwd();
drawBot();
}
else if(array[i-1] == 1 && array[i] == 3){
rotate180();
robHeading = 3;
moveFwd();
drawBot();
}
else if(array[i-1] == 2 && array[i] == 0){
rotate180();
robHeading = 0;
moveFwd();
drawBot();
}
else if(array[i-1] == 3 && array[i] == 1){
rotate180();
robHeading = 1;
moveFwd();
drawBot();
}
}
stopAllMotors();
//Indicate completion by spinning the spike, displaying a screen message,
//and playing an audio cue
spinSpikyThing();
eraseDisplay();
displayBigTextLine(3,"Maze Solved !!");
setSoundVolume(50);
playSound(soundDownwardTones);
playSound(soundUpwardTones);
playSound(soundDownwardTones);
playSound(soundUpwardTones);
sleep(2000);
stopAllMotors();
}
void spinSpikyThing(){
setMotorSpeed(armMotor, 100);
}
//drive the robot physically forward
void moveFwd(){
goFwd(); //advance the virtual robot forward to update GPS
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
setMotorSync(leftMotor, rightMotor, 1, 100);
moveMotorTarget(leftMotor, 506, 20);
moveMotorTarget(rightMotor, 506, 20);
waitUntilMotorStop(leftMotor);
waitUntilMotorStop(rightMotor);
}
//rotate robot physically rightward
void rotateRight(){
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
setMotorTarget(leftMotor, 147.5, 20);
setMotorTarget(rightMotor, -147.5, 20);
waitUntilMotorStop(leftMotor);
waitUntilMotorStop(rightMotor);
}
//rotate robot physically leftward
void rotateLeft(){
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
setMotorTarget(leftMotor, -147.5, 20);
setMotorTarget(rightMotor, 147.5, 20);
waitUntilMotorStop(leftMotor);
waitUntilMotorStop(rightMotor);
}
//for robustness, rotate robot 180° if the optimizer goes wrong for some reason
void rotate180(){
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
setMotorTarget(leftMotor, -295, 20);
setMotorTarget(rightMotor, 295, 20);
waitUntilMotorStop(leftMotor);
waitUntilMotorStop(rightMotor);
}
void stopAllMotors(){
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
setMotorSpeed(armMotor, 0);
}
Page updated
Google Sites
Report abuse