#! /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)
#! /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, 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)