States
A plan as a state variable, transitions, drawing the machine.
Do this lesson in the simulatorWandering has no plan. A patrol has one: go up, then right, then left, then down. The way a behaviour holds a plan is a state: one word that says which part of the plan you are in. Each tick does the right thing for the current state and checks whether it is time to change.
A state is a variable
state = "up"
That is all. The loop then looks like this:
if state == "up":
...do the up thing...
if ...the up thing is done...:
state = "right"
elif state == "right":
...
The program that results is called a state machine. Every washing machine, lift and traffic light is one.
Sliding to a point
The legs of this patrol are straight lines, and the robot can slide along them without turning. One tick of "slide towards a point while keeping heading 0":
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
# maths: atan2, hypot, sin, cos, radians
import math
def wrapped(h):
return (h + 180) % 360 - 180
def go_to(x, y, speed=70):
# where am I?
px, py = position()
a = math.radians(wrapped(math.degrees(math.atan2(x - px, y - py)) - heading()))
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed * math.cos(a), speed * math.sin(a), wrapped(0 - heading()) * 3)
def near(x, y, cm=6):
# where am I?
px, py = position()
return math.hypot(x - px, y - py) < cm
# do this 100 times (tick counts from 0)
for tick in range(100):
if near(0, 60):
# leave the loop
break
go_to(0, 60)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
print("at", position())
go_to is the vector driving from the parking project (lesson 3.6): the angle to the target, split into forward and sideways with cos and sin. near is the "am I done" test. Positions are relative to the start, and the start is the middle of zone A.
The machine
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
# maths: atan2, hypot, sin, cos, radians
import math
def wrapped(h):
return (h + 180) % 360 - 180
def go_to(x, y, speed=70):
# where am I?
px, py = position()
a = math.radians(wrapped(math.degrees(math.atan2(x - px, y - py)) - heading()))
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed * math.cos(a), speed * math.sin(a), wrapped(0 - heading()) * 3)
def near(x, y, cm=6):
# where am I?
px, py = position()
return math.hypot(x - px, y - py) < cm
route = {"up": (0, 60), "right": (60, 60)}
state = "up"
# do this 200 times (tick counts from 0)
for tick in range(200):
target = route[state]
if near(*target):
if state == "right":
# leave the loop
break
state = "right"
print("tick", tick, "now:", state)
else:
go_to(*target)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
print("at B:", position())
route maps each state to where that leg ends, so the loop body is the same for every state and only the transitions differ. *target unpacks the pair into two arguments.
Drawing it
Before writing a state machine, draw it: a circle per state, an arrow per transition, and on each arrow the condition. For this patrol: up, right, left, down, in a ring, each arrow labelled "near the corner". If the drawing is a mess, the code will be worse.
Task: patrol in states
From A to B and back, round the box: up, right, then left, down. Without touching the box.
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
# maths: atan2, hypot, sin, cos, radians
import math
def wrapped(h):
return (h + 180) % 360 - 180
def go_to(x, y, speed=70):
# where am I?
px, py = position()
a = math.radians(wrapped(math.degrees(math.atan2(x - px, y - py)) - heading()))
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed * math.cos(a), speed * math.sin(a), wrapped(0 - heading()) * 3)
def near(x, y, cm=6):
# where am I?
px, py = position()
return math.hypot(x - px, y - py) < cm
# do this 100 times (tick counts from 0)
for tick in range(100):
if near(0, 60):
# leave the loop
break
go_to(0, 60)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
Challenges
- Add a "pause" state: stop for one second at B before coming back.
- Patrol twice, with a lap counter.
- Make the LED a different colour in each state.