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}#####################")
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)
robot.sonar_calibrate(pi)
robot.sonar_calibrate(pi / 2)
robot.to_waypoint(126, 53)
robot.sonar_calibrate(-pi / 2)
count = 200
pos = (180, 42, 0)
robot = RobotBase(BrickPi3.PORT_B,
BrickPi3.PORT_C,
BrickPi3.PORT_D,
BrickPi3.PORT_1,
mymap,
p_start=pos,
p_count=count,
debug_canvas=canvas)
canvas.drawParticles(robot.p_tuples, robot.p_weights)
sleep(1)
# hijack points
print("particles start from (180,42,0), but robot is at (190,42,0)")
var = np.random.uniform(-20, 20, (count, 2))
hijack = []
for v in var:
hijack.append((pos[0] + v[0], pos[1] + v[1], pos[2]))
robot.p_tuples = hijack
canvas.drawParticles(robot.p_tuples, robot.p_weights)
sleep(1)
for _ in range(10):
robot.sonar_calibrate(20)
canvas.drawParticles(robot.p_tuples, robot.p_weights)
print(f"est pos: {robot.get_est_pos()}")
print("correct location based on sensor reading is 190,42")