Two loops at once

Heading and sideways controllers together on a holonomic robot.

3.5ControlRobot club20 min

Do this lesson in the simulator

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: LQR control explained

A BugBot can drive forward, slide sideways and rotate all at the same time, so it can run more than one controller at the same time. This lesson's robot curves and leaks sideways, and holding a line needs both.

Heading alone is not enough

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

def wrapped(h):
    return (h + 180) % 360 - 180

while position()[1] < 60:
    error = wrapped(0 - heading())
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(70, 0, error * 4)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()
# where am I? (cm from where I started)
x, y = position()
print(f"facing {round(heading())}, but x = {x}")

Run this in the simulator

The heading is held. The robot still ends up centimetres to the left, because it slides sideways even while pointing straight. Heading control cannot fix that.

Add a sideways controller

x from position() is the sideways error directly (the target is 0). Feed it to the sideways part of drive:

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

def wrapped(h):
    return (h + 180) % 360 - 180

while position()[1] < 60:
    # where am I? (cm from where I started)
    x, y = position()
    heading_error = wrapped(0 - heading())
    x_error = 0 - x
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(70, x_error * 10, heading_error * 4)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()
# where am I? (cm from where I started)
x, y = position()
print(f"facing {round(heading())}, x = {x}")

Run this in the simulator

Two errors, two gains, one drive. They do not fight, because rotation and sideways motion are separate on this robot. On a car they are not, which is why parallel parking is hard.

Tune them separately

Change one gain at a time. Set the sideways gain to 2 and watch it lag: 2 times a few centimetres is inside the dead band, so it ends at x = -6.7, no better than no sideways controller. Set it to 400 and watch it shake, jumping about half a centimetre either side of the line. Then do the same with the heading gain. Each has its own good range.

Holding a line that is not x = 0

To follow the line x = 10, the target changes and nothing else does:

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

def wrapped(h):
    return (h + 180) % 360 - 180

target_x = 10
while position()[1] < 60:
    # where am I? (cm from where I started)
    x, y = position()
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(70, (target_x - x) * 10, wrapped(0 - heading()) * 4)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()
print("ended at", position(), "facing", round(heading()))

Run this in the simulator

The robot slides over towards x = 10 in the first 20 cm or so and then holds it at about 8.5. The last 1.5 cm is the leak winning: at a gain of 10, 1.5 cm of error asks for only 15, right at the dead band. A higher sideways gain gets closer.

Task: on the rails

The zone is only 6 cm wide. Finish inside it, facing 0 within 5 degrees, on the curving, leaking robot.

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

def wrapped(h):
    return (h + 180) % 360 - 180

while position()[1] < 65:
    error = wrapped(0 - heading())
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(70, 0, error * 4)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()

Challenges

  1. Add a third controller: forward speed from the distance to the end of the zone, so it parks at y = 75 exactly.
  2. Follow the line x = -10, then x = 10, switching halfway.
  3. Write hold_line(target_x, until_y) as a function. You will want it in lesson 3.6.