Lab 6: Sensor Based Planning


Lead TAs:

Challenge Statement:

Find a way around an unknown map from a known start position to a known end position without touching any obstacle or leaving the map.


  1. The map will be discretized into an 8 x 16 grid with each block being 6' x 6'. All obstacles will be grid aligned and there will be enough space for a 6' x 6' robot to reach the goal from the start after padding all obstacles by one block. There are two stages to the lab :

    1. Mapping : The robot must follow the blue path shown below and mark all blocks inside the grey region that have an obstacle inside them. It is enough to mark only the obstacles that are clearly not occluded by any other from the robot's view point. You can pick any desired initial orientation but the robot must follow the path as indicated by the red arrows and come to a complete halt once it reaches the final blue block. You can demo it to a TA how many ever times you want before the due date.

    2. Planning : The robot must navigate the world with unknown, but grid aligned obstacles and reach the goal block from the start block. You can pick any desired initial orientation. The orientation of the robot at the goal position does not matter but the robot should come to a complete halt once it reaches it.
      Red : Start Position; Green : Goal Position; White : Obstacles will not be placed here; Grey : Obstacles may be placed here; Note that there will always be a path from the start to the goal such that the robot always has a one block radius around it free.The bottom left block of the map has coordinates (1, 1) and the top right (8, 16).

How to:

Note that the wave front planner is only suggested. Feel free to implement any other planner to solve this task.

Mapping :

  1. Attach an ultrasound sensor to the robot from the previous lab. Use a third motor to allow for panning the sensor.
  2. Write a function that takes an ultrasound reading, the position of the center of the robot and the orientation of the sensor and the robot and gives 2D coordinates of the point being probed. RobotC returns range values in cm which needs to be converted to inches and possibly offset by some amount to compensate for error.
  3. Rasterize these points and store them in a global array using the change_map function in the starter code. For example if you get a point with coordinates : (15.77 inches, 8.8 inches), you should mark the second row (from the bottom) and third column (from the left) of the array as an obstacle by calling change_map(2, 3, MARK).
  4. Write a function that makes the robot drive straight for 4 blocks, turn 90 degrees clockwise and drive straight for 5 more blocks. The ultrasound sensor must continuously pan in order to sweep 180 degrees taking at least 3 samples per sweep (one as far left as it can see, one as far right as it can see and one straight ahead).

Planning :

  1. Implementing the wave front algorithm that uses the global array from the mapping part of the lab.
  2. Use the wave front algorithm one block at a time :
      i) Scan and fill the array upto four blocks ahead and to the side
      ii) Plan the next move using the wave front algorithm
      ii) Make the move and do (i) again
    In other words - sense, plan and act, sense ...

Other tips:

  1. It is not necessary to use the kind of dead reckoning used in the previous labs as rotation and translation can be decoupled in the wave front algorithm and common motions (rotate 90 degrees or move one block forward) can be hardcoded.
  2. Be sure to mark the boundary of the map as an obstacle for the wave front algorithm.
  3. When panning make sure to stop and wait for a few milliseconds before polling the sensor.
  4. Take multiple range readings and average them for better results.
  5. There are ranges above or which the sensor performs very poorly. Make sure to ignore points in that range. A good operating range for the LEGO ultrasound sensor is 2 - 20 inches.
  6. If two or more robots are close to each other, sensor readings may be spurious.

Sample starter code : Code for visual display of an 8 x 16 2D array.


  1. Mapping (50 points) :

    1. Robot seems to mark some blocks and move correctly - 20 points
    2. Robot executes motion correctlly but marks more than 2 but less than 5 blocks wrong - 10 points
    3. 1 or 2 blocks wrongly marked / not marked - 20 points
    4. All blocks correctlly marked - 30 points

  2. Planning (50 points) :

    1. Robot seems to sense, plan and act - 10 points
    2. Robot hits an obstacle after travelling between 5 to 10 unique blocks - 20 points
    3. Robot hits an obstacle after travelling between 10 to 20 unique blocks - 30 points
    4. Robot stops within two blocks of the goal - 40 points

Grading Sheet : Lab 6

Keywords : Rasterization, Wave front algorithm, Ultrasound

Last Updated : 10/16/07 by Kaushik Viswanathan
(c) 1999-2007: Howie Choset, Carnegie Mellon