async def step_8(robot1, robot2, currentPos1, currentPos2):
# Undo the movements made by the robots when trying to 'look natural' in Step 6.
await robot1.turn_in_place(degrees(-70)).wait_for_completed()
await robot1.drive_straight(distance_mm(-150),
speed_mmps(50)).wait_for_completed()
# Navigation procedure as in Step 2.
# ROBOT 1
a = AStar(robot1, currentPos1)
print('Navigating from:', (4, 2), 'to', (0, 2))
print('')
path = a.initLegoWorld((4, 2), (0, 2))
for i in range(len(path)):
await a.move(path[i])
currentPos1 = a.getPos()
# ROBOT 2
a = AStar(robot2, currentPos2)
print('Navigating from:', (4, 0), 'to', (0, 6))
print('')
path = a.initLegoWorld((4, 0), (0, 6))
for i in range(len(path)):
await a.move(path[i])
await a.face("south")
currentPos2 = a.getPos()
return True, currentPos1, currentPos2
async def step_4(robot1, robot2, currentPos1, currentPos2, destPos):
# Navigation procedure as in Step 2.
# ROBOT 1
a = AStar(robot1, currentPos1)
print('Navigating from:', (4, 6), 'to', (8, 4))
print('')
path = a.initLegoWorld((4, 6), (8, 4))
for i in range(len(path)):
await a.move(path[i])
await a.face("north")
currentPos1 = a.getPos()
# ROBOT 2
a = AStar(robot2, currentPos2)
print('Navigating from:', (2, 6), 'to', (6, 4))
print('')
dest = (destPos[0], destPos[1])
path = a.initLegoWorld((2, 6), dest)
for i in range(len(path)):
await a.move(path[i])
await a.face("east")
currentPos1 = a.getPos()
return True, currentPos1, currentPos2
async def step_6(robot1, robot2, currentPos1, currentPos2):
# Navigation procedure as in Step 2.
# ROBOT 2
a = AStar(robot2, currentPos2)
print('Navigating from:', (6, 4), 'to', (4, 0))
print('')
path = a.initLegoWorld((6, 4), (4, 0))
for i in range(len(path)):
await a.move(path[i])
await a.face("south")
currentPos2 = a.getPos()
# ROBOT 1
a = AStar(robot1, currentPos1)
print('Navigating from:', (8, 4), 'to', (4, 2))
print('')
path = a.initLegoWorld((8, 4), (4, 2))
for i in range(len(path)):
await a.move(path[i])
currentPos1 = a.getPos()
await robot1.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
await robot1.turn_in_place(degrees(70)).wait_for_completed()
return True, currentPos1, currentPos2
async def step_2(robot1, currentPos1):
# Use an instance of the 'astar' class to align the robots
a = AStar(robot1, currentPos1)
print('Navigating from:', (6, 4), 'to', (4, 6))
print('')
# Give start (A) and end (B) points to the pathfinder
# Follow the path that is returned
# This code appears several times, and can almost certainly be made into
# a discrete function, but this works well for now.
path = a.initLegoWorld((6, 4), (4, 6))
for i in range(len(path)):
await a.move(path[i])
# Robot 1 needs to face Robot 2
await a.face("north")
currentPos = a.getPos()
return True, currentPos
