def TEST2():
brickButton = Button()
rfidReader = RfidSerialThread("COM5" if (platform.system() == "Windows") else "/dev/ttyUSB0")
print("FOLLOWING LINES, DETECTING TAGS:")
rfidReader.start()
sleep(4)
robot.resetOrientation()
while not brickButton.any():
robot.followLine(200)
if rfidReader.getCurrentTagId() is not None:
robot.stopMotors()
print("TAG FOUND - id", rfidReader.getCurrentTagId())
Sound.beep()
sleep(1)
robot.runMotorsDistance(-5.7, 150)
sleep(4)
robot.runMotorsDistance(12.0, 150) #to go past the tag
if (robot.getDistanceAhead() is not None and robot.getDistanceAhead() < 20):
print("Obstacle! Dist =", robot.getDistanceAhead(), "Exiting...")
break
robot.stopMotors()
rfidReader.stop()
def Main():
map_var = Map(Ghost_mapping_type.none)
io = IO()
logic = Logic(map_var)
button = Button()
saver = Map_saver(map_var, button)
saver.wait_for_load()
print("READY")
motors = {
Moves.fwd: lambda: io.go_forward(),
Moves.left: lambda: io.go_left(),
Moves.right: lambda: io.go_right(),
Moves.back: lambda: io.go_back()
}
mapping = {
Moves.fwd: lambda: map_var.go_forward(),
Moves.left: lambda: map_var.go_left(),
Moves.right: lambda: map_var.go_right(),
Moves.back: lambda: map_var.go_back()
}
motors[Moves.fwd]()
mapping[Moves.fwd]()
while (True):
clr_sensor = io.directions_free()
us_sensor = io.ghost_distance()
map_var.write_sensor_values(clr_sensor, us_sensor)
debug_print(map_var)
move = logic.get_next_move()
debug_print(map_var)
switch[move]()
if button.any():
saver.wait_for_load()
ok = motors[move]()
if (ok is False):
io.after_crash()
map_var.rotation += int(move)
continue
elif (ok is None):
saver.wait_for_load()
continue
mapping[move]()
saver.save_map()