Turns and waypoints

The robot keeps track of where it is at all times. In this lesson you learn to use that: turn to an exact heading and drive by coordinates.

How the robot knows where it is

From the wheel encoders the robot keeps recomputing three numbers — the same ones shown on the screen: X and Y in millimetres and Angle in degrees. This is called odometry.

+Xforward+Yright+anglestart: X = 0, Y = 0, angle = 0°
The X axis points forward from the starting pose, the Y axis points right. A positive angle is a turn to the right.
  • At power-on the robot assumes it stands at (0, 0) facing along the X axis.
  • The current values are returned by getAbsX(), getAbsY() and getAbsAngle(), and all three at once by getOdometry().

Steps

  1. Compare the two turns

    robot.rotate(50, 90);     // another 90 degrees to the right of where the robot faces now
    robot.rotateTo(50, 90);   // face heading 90 degrees, wherever it faces now

    rotate() turns by an angle counted from the previous target, so in a series of moves the errors do not pile up. rotateTo() turns to a heading of the odometry and does not depend on what came before: it also works after motors(), stop() or after the robot was pushed.

  2. Drive a square on absolute headings

    robot.setPosition(0, 0, 0);
    for (int i = 0; i < 4; i++) {
      robot.moveDist(50, 300);
      robot.rotateTo(50, (i + 1) * 90);   // headings 90, 180, 270, 360
    }
    robot.displayPrint("Angle", robot.getAngle());

    At the end the screen shows the accumulated angle. The closer it is to 360, the more exactly the robot has returned to its original direction.

  3. Tie the coordinates to the field

    robot.setPosition(0, 0, 0);   // x, y in millimetres and heading in degrees

    setPosition() tells the robot where it stands. You can give the real coordinates of the start cell — for example, robot.setPosition(150, 150, 90).

  4. Describe a route as points

    const float route[][2] = {
      {400, 0},
      {400, 400},
      {0, 400},
      {0, 0},
    };
    
    void setup() {
      robot.begin();
      robot.setPosition(0, 0, 0);
    
      for (int i = 0; i < 4; i++) {
        robot.moveTo(50, route[i][0], route[i][1]);
        delay(300);
      }
      robot.rotateTo(50, 0);   // restore the original heading at the end
    }
    XY1234(0, 0)(400, 0)(400, 400)(0, 400)
    A route of four points. moveTo() turns the robot towards the target by itself and drives to it in a straight line.
  5. Watch the position from the computer

    void loop() {
      robot.printOdometry();
      delay(1000);
    }

    Open the Serial Monitor at 115200 baud. Once a second it prints a line with the encoder ticks, the coordinates and the heading.

Finished sketches: File › Examples › UNI › PrecisionTurns and DriveToPoint. The code is on the Examples page.

What should happen

(400, 0)(400, 400)(0, 400)(0, 0)start and finishrotateTo(50, 0)at each point itturns on the spota 0.3 s pauserotateTo(50, 0)at the end: facingwhere it startedSerial Monitor, 115200 baud — once a second after the finishencL: 9433 encR: 6185 X: 1.84 Y: -2.31 Theta: 0.62encL: 9433 encR: 6185 X: 1.84 Y: -2.31 Theta: 0.62
The robot visits the four points: at each it waits 0.3 s and turns on the spot towards the next, and at (0, 0) rotateTo turns it back to the heading it started with. Lines appear in the Serial Monitor once the route is done: loop() only starts after setup().

Try changing it

  • Describe a triangle or a U shape with points.
  • Run the PrecisionTurns example: it drives a square first with rotate(), then with rotateTo(), and shows the angle after each run. Compare the two numbers.
  • Stretch the route to ten points and watch how the return error grows.

If it does not work

  • The robot drove the wrong way. Check the axes figure: +Y is to the right. Check the argument order: moveTo(power, x, y).
  • The robot does not return exactly to the start. Odometry error grows with distance and with wheel slip. The reasons are explained in Accuracy and Odometry.
  • The coordinates did not reset after resetAngle(). resetDistance() and resetAngle() only clear the distance and angle counters. The pose is set by setPosition().