# navigate to Area B
robot.touch_bottle(500)
# go to AREA A
robot.to_relative_turn(radians(180))
robot.to_waypoint(126, 42)
robot.sonar_calibrate(0)
robot.to_relative_turn(radians(90))
#####################
# AREA A #
#####################
# navigate to origin
robot.touch_bottle(500)
robot.sonar_calibrate(-pi / 2)
# go to checkpoint 2
# robot.to_relative_turn(atan2(12,84))
# robot.to_relative_backward(85) # sqrt( 12**2 + 84**2 ) .= 85
o_obs = robot.get_nearest_obstacles(*cfg_scan_b_range, cfg_scan_b_var, True)
o_bottle, o_walls = robot.identify_bottle(o_obs)
_x, _y = robot.update_pos(o_walls)
print(f"Origin update: X{_x}, Y: {_y}")
robot.to_waypoint(84, 30)
# if calibrate_and_getbottle():
# return True
if robot.to_waypoint(42, 106):
return True
if calibrate_and_getbottle():
return True
return False
#####################
# Main #
#####################
areaB()
areaA()
areaC()
robot.to_waypoint(84, 30)
c_calibrate_obs = robot.get_nearest_obstacles(-pi, pi, radians(2), DEBUG=True)
walls_likelyhood = []
_, walls = robot.identify_bottle(c_calibrate_obs,
walls_likely=walls_likelyhood)
robot.update_pos(walls, walls_likelyhood)
robot.to_waypoint(84, 30)