canvas.drawParticles(robot.p_tuples, robot.p_weights)
cfg_scan_a_range = (radians(-30), radians(30), radians(2))
cfg_scan_b_range = (radians(-30), radians(-200), radians(-2))
cfg_scan_a_var = 0.4
cfg_scan_b_var = 0.4
#####################
# AREA C #
#####################
# navigate to Area B
robot.to_waypoint(42, 63)
robot.set_motor_position(robot.M_SONAR, 130)
original_theta = atan2(42, 33)
robot.to_relative_turn(-original_theta)
robot.sonar_calibrate(pi / 2)
robot.sonar_calibrate(pi)
# a_bottle, a_walls = robot.identify_bottle(a_obs)
# _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)
# 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)
# d: D to E
# e: E to F
# f: F to G
# g: G to H
# h: H to O
mymap.add_wall((0,0,0,168)); # a
mymap.add_wall((0,168,84,168)); # b
mymap.add_wall((84,126,84,210)); # c
mymap.add_wall((84,210,168,210)); # d
mymap.add_wall((168,210,168,84)); # e
mymap.add_wall((168,84,210,84)); # f
mymap.add_wall((210,84,210,0)); # g
mymap.add_wall((210,0,0,0)); # h
canvas.drawLines(mymap.walls)
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 = [(84.0,30),(180,30), (180,54), (138,54), (138,168), (114,168), (114,84), (84,84), (84,30)]
canvas.drawParticles(robot.p_tuples, robot.p_weights)
print(f"location {robot.get_est_pos()}")
sleep(1)
for i, waypoint in enumerate(waypoints):
robot.to_waypoint(*waypoint)
canvas.drawParticles(robot.p_tuples, robot.p_weights)
print(f"location {robot.get_est_pos()}")
print(f"#################finish way point {i+1}#####################")
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}#####################")