Home / Programming / ROBOTC / Robot Virtual Worlds – Maze Crawler

Robot Virtual Worlds – Maze Crawler

It’s finally finished and ready to be published, my Robot Virtual Worlds Maze Crawler is fully functional.  Check out the screenshot below.


The engine for this robot consists of two Finite State Machines (FSM) that handle the cruising around, turning, scanning, etc.  Here’s a little video of it, doing its thing.

Main FSM

Main Finite State MachineThe main state machine consists of 5 states:

    • Initialise the maze tiles
    • Set the walls of the perimeter
    • Set the pointers to the finish tile and current tile
  • SCAN
    • Check for a wall ahead and fix the robot’s position if required
    • Scan left and right walls if they have not been scanned already
    • Update current tile walls and those of the neighbouring tiles
  • TURN
    • Call the route planning function and get new heading
    • Change robot’s heading and wait for turn to complete
    • Reset the encoders
    • Wait until the robot gets to the centre of a new tile
    • Update the position of the robot and update the current tile pointer
    • Check if the new tile is the finish tile, stop if this is true
  • END
    • Stop the robot, exit the main thread

Scanning FSM

Scan Finite State Machine

The scanning FSM has 6 states:

    • Scan the wall ahead and update its status and of adjacent tile
    • Check if left and right wall are known and skip directly to the end
    • Check if left wall is know and skip directly to right wall
    • Change the heading of the robot and wait for turn to complete
    • Scan the wall on the left and update the status of this wall and of adjacent tile
    • Check if the right wall needs to be scanned, if not, go to the end
    • Change the heading of the robot and wait for turn to complete
    • Scan the wall on the right and update the status of this wall and of adjacent tile
  • END
    • This state is only used to exit the FSM

Route planner

The current route planner is very simple, it’s a “follow the right wall” and will only work with certain mazes.  The robot uses the following algorithm to decide where to go next after it’s done scanning a tile:

  • Turn using the following priority list:
    • Turn right
    • Go straight
    • Turn left
    • Turn 180 degrees

That’s it.  Nothing more to it.

Modifying the route planner

If you wanted to experiment with different route planners, simply change the function called in doMainStateTurn(), which gets called each time we get to another tile.

* Here we see which way we'll go next.  The actual decision is made 
* by a route planning function, routePlannerFollowWall() in this case 
void doMainStateTurn() 

  int dir = headingToDirection(currentHeading); 
  int newHeading = requiredHeading;

  // Get a new heading from the route planning function 
  newHeading = routePlannerFollowWall(dir, maze, currentTile);

  // Don't bother to do anything if the newHeading is the same 
  // as the current one. 
  if (newHeading == requiredHeading) 

  writeDebugStreamLine("doMainStateTurn: dir: %d, curr: %d, new: %d", dir, currentHeading, newHeading); 
  updateMotors(TURN_SPEED, newHeading);

  // wait for the turning to be completed. 
  while (abs(currentRelativeHeading) > 5) 


  // Go to the next state 

Future improvements

Things that would be cool to add:

  • A route planner that makes sure that all tiles are discovered and mapped.
  • A route planner that will work with mazes that are not perfect
  • A solver that will find its way back to the start using the fastest route

Download the code

You can get the code and the RVW level here in a single zip file.  Make sure you select the “Mammalbot” as your robot or it won’t work.

About Xander

Xander Soldaat is a Software Engineer and former Infrastructure Architect. He loves building and programming robots. He recently had the opportunity to turn his robotics hobby into his profession and has started working for Robomatter, the makers of ROBOTC and Robot Virtual Words.