# _x,_y = robot.update_pos(a_walls)
# print(f"update: X{_x}, Y: {_y}")
robot.touch_bottle(500)
# go to AREA B
robot.to_relative_backward(10)
robot.sonar_calibrate(pi)
robot.sonar_calibrate(pi / 2)
robot.to_waypoint(126, 53)
robot.sonar_calibrate(-pi / 2)
robot.to_waypoint(126, 100)
robot.sonar_calibrate(-pi / 2)
_, _, _t = robot.get_pos_mean()
robot.to_relative_turn(_t - pi / 2)
#####################
# AREA B #
#####################
# navigate to Area B
robot.touch_bottle(500)
# go to AREA A
robot.to_relative_turn(radians(180))
robot.to_waypoint(126, 42)
name_list = ["a","b","c","d","e","f","g","h"]
# to waypoint
robot = RobotBase(BrickPi3.PORT_B, BrickPi3.PORT_C,BrickPi3.PORT_D, BrickPi3.PORT_4, mymap, p_start=(84.0,30.0,0), debug_canvas=canvas)
waypoints = [(180,30), (180,54), (138,54), (138,168), (114,168), (114,84), (84,84), (84,30)]
calpoints = [(100,30),(120,30),(140,30),(160,30)]
canvas.drawPoints(waypoints)
canvas.drawParticles(robot.p_tuples, robot.p_weights)
print(f"location {robot.get_pos_mean()}")
sleep(1)
for cal in calpoints:
robot.to_waypoint(*cal, accuracy=10)
canvas.drawParticles(robot.p_tuples, robot.p_weights, robot.get_pos_mean())
sleep(1)
while robot.get_pos_var()[1] > 5:
robot.sonar_calibrate(-0.5*pi)
canvas.drawParticles(robot.p_tuples, robot.p_weights, robot.get_pos_mean())
sleep(1)
for i, waypoint in enumerate(waypoints):
robot.to_waypoint(*waypoint)
sleep(2)
canvas.drawParticles(robot.p_tuples, robot.p_weights, robot.get_pos_mean())
print(f"location {robot.get_pos_mean()}")
print(f"#################finish way point {i+1}#####################")