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()