#! /usr/bin/python3
from brickpi3 import BrickPi3
from helper import RobotBase, Canvas, Map
from time import sleep
from math import pi
canvas = Canvas(210)
mymap = Map()
# 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)
robot.to_relative_forward(100)
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"]
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)
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 #
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)
canvas.drawParticles(robot.p_tuples, robot.p_weights)
##################################
# FIND AREA A FROM STARTPOINT #
##################################
# scan bottle
cfg_a_start = radians(-180)
cfg_a_step = radians(1)
cfg_a_end = radians(180)
# 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}#####################")
#! /usr/bin/python3
from brickpi3 import BrickPi3
from helper import RobotBase, Canvas, Map
from time import sleep
from math import pi
robot = RobotBase(BrickPi3.PORT_B, BrickPi3.PORT_C)
canvas = Canvas(40)
mymap = Map()
# draw wall
wall_corner = [(0,0), (0,40), (40,40), (40,0)]
for i in range(3):
mymap.add_wall((*wall_corner[i], *wall_corner[i+1]))
mymap.add_wall((*wall_corner[3], *wall_corner[0]))
# draw lines
canvas.drawLines(mymap.walls)
# init
for _ in range(4):
for _ in range(4):
robot.to_relative_forward(10) # *task unpack to arguments
canvas.drawParticles(robot.p_tuples, robot.p_weights)
print (f"est postion:{robot.get_est_pos()}")
sleep(1)
robot.to_relative_turn(0.5*pi)
# 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"]
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)
canvas.drawParticles(robot.p_tuples, robot.p_weights)
cfg_scan_a_range = (radians(-180), radians(180), radians(2))
def calibrate_and_getbottle():
obstacles = robot.get_nearest_obstacles(-pi, pi, radians(2), DEBUG=True)
walls_likelyhood = []
_, walls = robot.identify_bottle(obstacles, walls_likely=walls_likelyhood)
robot.update_pos(walls, walls_likelyhood)
# 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 = [(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)
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"]
# init
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)