pid_config_file="carpet_config.json",
Map=MAP,
mcl=True,
x=86,
y=30,
mode="continuous",
theta=0,
threading=True,
canvas=Canvas)
Map.add_wall((0, 0, 0, 168)) # a
Map.add_wall((0, 168, 84, 168)) # b
Map.add_wall((84, 126, 84, 210)) # c
Map.add_wall((84, 210, 168, 210)) # d
Map.add_wall((168, 210, 168, 84)) # e
Map.add_wall((168, 84, 210, 84)) # f
Map.add_wall((210, 84, 210, 0)) # g
Map.add_wall((210, 0, 0, 0)) # h
Map.draw()
for point in POINTS:
Canvas.drawCross(point)
for x, y in POINTS:
Robot.step_to_waypoint(x, y, maxdistance=20)
PARTICLES = Robot.get_state()
Canvas.drawParticles(PARTICLES)
Robot.stop_threading()
interface.terminate()
#Initialize the interface
interface=brickpi.Interface()
interface.initialize()
NUMBER_OF_PARTICLES = None
OFFSET = 100
DISTANCE_TO_PIXEL = 15
#Draw the square
print "drawLine:" + str((OFFSET,OFFSET,40*DISTANCE_TO_PIXEL+OFFSET,OFFSET))
print "drawLine:" + str((40*DISTANCE_TO_PIXEL+OFFSET,OFFSET,40*DISTANCE_TO_PIXEL+OFFSET,OFFSET+40*DISTANCE_TO_PIXEL))
print "drawLine:" + str((40*DISTANCE_TO_PIXEL+OFFSET,40*DISTANCE_TO_PIXEL+OFFSET,OFFSET,40*DISTANCE_TO_PIXEL+OFFSET))
print "drawLine:" + str((OFFSET,40*DISTANCE_TO_PIXEL+OFFSET,OFFSET,OFFSET))
Robot = Robot(interface, pid_config_file="carpet_config.json")
for i in range(4):
for j in range(4):
Robot.travel_straight(10,update_particles=True)
NUMBER_OF_PARTICLES = Robot.get_state()
NUMBER_OF_PARTICLES = [(OFFSET+point[0][0]*DISTANCE_TO_PIXEL,OFFSET+point[0][1]*DISTANCE_TO_PIXEL,point[0][2]) for point in NUMBER_OF_PARTICLES]
print "drawParticles:" + str(NUMBER_OF_PARTICLES)
time.sleep(5)
Robot.rotate_right(90,update_particles=True)
NUMBER_OF_PARTICLES = Robot.get_state()
NUMBER_OF_PARTICLES = [(OFFSET+point[0][0]*DISTANCE_TO_PIXEL,OFFSET+point[0][1]*DISTANCE_TO_PIXEL,point[0][2]) for point in NUMBER_OF_PARTICLES]
print "drawParticles:" + str(NUMBER_OF_PARTICLES)
time.sleep(5)
interface.terminate()
