How a robot drives itself to a waypoint

Waypoint navigation gives a robot a target point, an x and a y, instead of a list of turns. On every pass of the loop it compares where it thinks it is, from dead reckoning, with where it's going, picks the direction that closes the gap, turns to face it if needed, and drives. A list of waypoints makes a mission. No route is stored anywhere: it falls out of the answers.

The rule

On a grid where the rover faces north, east, south or west:

1. Arrived? If x and y both match the waypoint, move on to the next one, or stop at the last.
2. Level up y first: want north if y is below the waypoint's, south if above.
3. Then x: east if x is below, west if above.
4. Face, then drive: if it isn't facing that way, pivot; otherwise drive, stepping x and y on every encoder tick.

From (0, 0) to (2, 5): five ticks north, a quarter turn right, two ticks east.

In code

if (x == wx[wp] && y == wy[wp]) {
  if (wp < last) wp = wp + 1;             // on to the next waypoint
  else { stopWheels(); digitalWrite(13, HIGH); return; }   // ARRIVED
}
int want;
if (y < wy[wp]) want = 0;                 // north
else if (y > wy[wp]) want = 2;            // south
else if (x < wx[wp]) want = 1;            // east
else want = 3;                            // west

Then, if dir isn't want, pivot right and count ticks: every 10 is a quarter turn, dir = (dir + 1) % 4. Otherwise drive, and on each tick add dx[dir] and dy[dir] to x and y.

Live experiment

Send the rover to a waypoint

The rover decides where to go on every encoder tick: north or south until level, then east or west, pivoting a quarter turn of 10 ticks when it needs to face a new way. Send it to the flag and the dock, then behind it, and try counting a quarter turn as 5 ticks.

Try it yourself: free lesson

“Go to a waypoint” is lesson 4 of 6 in Waypoint Wharf, Robotics World 3. Free with an account. Lessons unlock in order, so you’ll do the 17 before it first.

It only knows what it counts

The rover's position is a sum of ticks. Count a quarter turn as 5 ticks instead of 10 and each turn is only about 45°: the rover drives off at an angle, its sums still say it reached the waypoint, and it lights ARRIVED somewhere else. Wheel slip does the same, more slowly. Real robots correct dead reckoning with a gyro, beacons or GPS.

Beyond the grid

This rule is greedy: it always heads straight for the goal, so a wall in the way would stop it. Path planners such as A* search a map for the best way round obstacles, then hand the robot that route as a list of waypoints. Free-roaming robots steer toward the waypoint's bearing, atan2(dy, dx), instead of four grid directions. Drone flight plans, tractors crossing fields and pavement delivery robots all run on waypoint lists.