def __init__(self):
threading.Thread.__init__(self)
self.manager = SimulationManager()
self.client_id = self.manager.launchSimulation(gui=True)
p.connect(p.DIRECT)
self.robot = Robot(self.manager, self.client_id)
self.createScene()
self.initUI()
def setup_class(self, tmpdir):
# self.ROOT_DIR = os.path.dirname(os.path.abspath(__file__))
self.ROOT_DIR = tmpdir.strpath
self.USER_CONFIG = self.ROOT_DIR + '/dockers/user_config.json'
os.makedirs(self.ROOT_DIR + "/dockers")
with open(self.USER_CONFIG, 'w') as f:
f.write("{}")
self.robot = Robot(root_dir=self.ROOT_DIR,
user_config=self.USER_CONFIG,
domain="tests.com")
self.robot.domain = "tests.com"
def do(self, grid: Grid, robot: Robot):
if robot.is_lost:
raise Exception('Dead')
orientation = robot.orientation
current_pos = robot.position
# Ignore instruction if known bad place.
if MoveForward.__to_dropoff(grid, current_pos, orientation):
return
next_position = orientation.next_forward_position(current_pos)
if grid.is_within_grid(next_position):
robot.set_position(next_position)
else:
robot.is_now_lost()
label = Label(orientation)
grid.add_scent(current_pos, label)
def run_cli():
cli_robot = Robot()
print "Enter your robot command (press q to quit):"
while True:
shell_command = raw_input('>')
shell_command = shell_command.strip().lower()
if not shell_command:
continue
if shell_command == 'q':
break
words = shlex.split(shell_command)
# Grab the arguments
args = words[1:] if len(words) > 1 else []
# Grab the method
method_name = words[0]
try:
method = getattr(cli_robot, words[0])
result = method(*args)
if result:
print "The coordinate of the robot is (%s, %s) and facing %s" % result
except:
print "Class {} does not implement {} command".format(
cli_robot.__class__.__name__, method_name.upper())
def test_read_instructions_three_robots_oneloss_oneavoidance(self):
dir_name = self._create_dir()
filepath = f'{dir_name}/allgood'
afile = (
f'{filepath}_2',
'''5 3\n\n1 1 E\nRFRFRFRF\n\n3 2 N\nFRRFLLFFRRFLL\n\n0 3 W\nLLFFFLFLFL\n'''
)
expectations = (
('::Start[1,1,E] ::TurnRight ::MoveForward ::TurnRight ::MoveForward ::TurnRight ::MoveForward ::TurnRight ::MoveForward',
'1 1 E'),
('::Start[3,2,N] ::MoveForward ::TurnRight ::TurnRight ::MoveForward ::TurnLeft ::TurnLeft ::MoveForward ::MoveForward ::TurnRight ::TurnRight ::MoveForward ::TurnLeft ::TurnLeft',
'3 3 N LOST'),
('::Start[0,3,W] ::TurnLeft ::TurnLeft ::MoveForward ::MoveForward ::MoveForward ::TurnLeft ::MoveForward ::TurnLeft ::MoveForward ::TurnLeft',
'2 3 S'),
)
with open(afile[0], 'a') as fl:
fl.write(afile[1])
inst_file_processor = InstructionsFile(afile[0])
inst_file_processor.initialise_instructions()
grid_extents = inst_file_processor.grid_extents
self.assertEqual(grid_extents.coord_x, 5)
self.assertEqual(grid_extents.coord_y, 3)
grid = Grid(grid_extents)
count = 0
for robot_instruction in inst_file_processor.next_instructions():
self.assertEqual(str(robot_instruction), expectations[count][0])
robot = Robot()
for inst in robot_instruction.instruction_list:
inst.do(grid, robot)
if robot.is_lost:
break
self.assertEqual(str(robot), expectations[count][1])
count += 1
self.assertEqual(count, 3)
def get_robots_output(self):
""" Given a set of commands and plateau dimensions parses this values
run robot commands, and yield the current coordinates and
orientations.
"""
while self.commands:
coordinates_and_orientation = self.commands.pop(0)
coordinates = list(
map(int,
coordinates_and_orientation.split(' ')[:2]))
orientation = coordinates_and_orientation.split(' ')[-1]
robot_commands = self.commands.pop(0)
robot = Robot(coordinates, orientation, self.plateau_dimensions)
[robot.run(command) for command in robot_commands]
yield robot.get_coordinate_and_orientation_text()
class TestClass:
garage = Garage(3)
# Create the robots
r2d2 = Robot("R2D2", 20, 40, 0, "Anakin")
c3po = Robot("C3PO", 30, 50, 0, "Vader")
bb8 = Robot("BB8", 10, 45, 0, "Rei")
# Park the robots
garage.park_robot(r2d2)
garage.park_robot(c3po)
garage.park_robot(bb8)
# Create procedures
procedures = [
Procedure(50, robots=[r2d2, bb8]),
Procedure(60, robots=[r2d2, c3po]),
Procedure(100, robots=[r2d2, c3po, bb8]),
Procedure(150, robots=[bb8]),
Procedure(200, robots=[r2d2])
]
# Test the capacity of the created garage
def test_garage(self):
assert self.garage.capacity == 3
# Test the robots actions and the manage
# of the energy and happiness
def test_robot_move(self):
self.r2d2.move()
assert self.r2d2.energy == 39 and self.r2d2.happiness == 19
def test_robot_work(self):
self.c3po.work()
assert self.c3po.energy == 49 and self.c3po.happiness == 31
def test_robot_hibernate(self):
self.bb8.hibernate()
assert self.bb8.happiness == 9 and self.bb8.energy == 46
# Test that the parked robots are in the garage
def test_robot_list(self):
flag = True
for i in self.garage.robots:
print(i)
assert flag == True
def test_right(self, mock_calc_right_transition, mock_is_within_boundary):
mock_is_within_boundary.return_value = True
mock_calc_right_transition.return_value = EAST
test_robot = Robot()
test_robot.place(1, 1, NORTH)
test_robot.right()
result = test_robot.report()
self.assertEqual((1, 1, EAST), result)
def test_move(self, mock_calc_x_y_vec, mock_is_within_boundary):
mock_is_within_boundary.return_value = True
mock_calc_x_y_vec.return_value = (0, 1)
test_robot = Robot()
test_robot.place(1, 1, NORTH)
test_robot.move()
result = test_robot.report()
self.assertEqual((1, 2, NORTH), result)
def populate_data(self):
self.state.garage = Garage(3)
# Create the robots
r2d2 = Robot("R2D2", 20, 40, 0, "Anakin")
c3po = Robot("C3PO", 30, 50, 0, "Vader")
bb8 = Robot("BB8", 10, 45, 0, "Rei")
# Park the robots
self.state.garage.park_robot(r2d2)
self.state.garage.park_robot(c3po)
self.state.garage.park_robot(bb8)
# Create procedures
self.state.procedures = [
Procedure(50, robots=[r2d2, bb8]),
Procedure(60, robots=[r2d2, c3po]),
Procedure(100, robots=[r2d2, c3po, bb8]),
Procedure(150, robots=[bb8]),
Procedure(200, robots=[r2d2])
]
class Simulator:
ORIGIN = Point(0, 0)
def __init__(self):
self.robot = Robot(Simulator.ORIGIN, WindRose.NORTH_INDEX)
self.grid = Grid()
def run(self, steps):
step = 0
if steps < 0:
raise Exception('number of steps must be positive(%d)' % steps)
while step < steps:
print('Step %d' % step)
original_cell = self.robot.position
cell = self.grid.add_cell(original_cell)
if cell.color == Color.WHITE:
self.robot.clockwise_rotate()
elif cell.color == Color.BLACK:
self.robot.anti_clockwise_rotate()
cell.flip()
self.robot.move()
step = step + 1
def export_grid(self, filename):
print('export_grid')
print(self.grid.x_range)
print(self.grid.y_range)
for cell in self.grid.cells:
print('cell: %s - color:%s' % (cell, self.grid.cells[cell].color))
x_min = self.grid.x_range[0] - 2
x_max = self.grid.x_range[1] + 2
y_min = self.grid.y_range[0] - 2
y_max = self.grid.y_range[1] + 2
with open(filename, "w") as simulation_file:
simulation_file.write('x range: [%d,%d]\n' % (x_min, x_max))
simulation_file.write('y range: [%d,%d]\n' % (y_min, y_max))
simulation_file.write('\n')
y = y_max
while y >= y_min:
simulation_file.write('|')
x = x_min
while x <= x_max:
cell = self.grid.cells.get(Point(x, y))
if cell is None or cell.color == Color.WHITE:
simulation_file.write('W|')
elif cell.color == Color.BLACK:
simulation_file.write('B|')
x = x + 1
y = y - 1
simulation_file.write('\n')
def test_sense():
world_size = 10.0 # size of world (square)
measurement_range = 5.0 # range at which we can sense landmarks
motion_noise = 0.2 # noise in robot motion
measurement_noise = 0.2 # noise in the measurements
# instantiate a robot, r
rob = Robot(world_size, measurement_range, motion_noise, measurement_noise)
num_landmarks = 3
rob.make_landmarks(num_landmarks)
# print out our robot's exact location
print(rob)
# display the world including these landmarks
display_world(int(world_size), [rob.x, rob.y], rob.landmarks)
# print the locations of the landmarks
print('Landmark locations [x,y]: ', rob.landmarks)
measurements = rob.sense()
assert len(measurements) == 3, "rob did not sense all 3 landmarks"
print(measurements)
def run_main(self):
parsedargs = self.buildArgParser()
input_file = parsedargs.input_file
print(f'===== {input_file} =====')
inst_file_processor = InstructionsFile(input_file)
try:
inst_file_processor.initialise_instructions()
grid = Grid(inst_file_processor.grid_extents)
for robot_instructions in inst_file_processor.next_instructions():
robot = Robot()
self.do_instructions(grid, robot, robot_instructions)
print(str(robot))
print()
except ExceptionFileParseCritical as ex:
print(ex)
print(ex.path)
if ex.line_num is not None:
print(f'@{ex.line_num}: "{ex.line}"')
print()
exit(ex.code)
def make_data(N, num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise, distance):
# check if data has been made
complete = False
while not complete:
data = []
# make robot and landmarks
r = Robot(world_size, measurement_range, motion_noise,
measurement_noise)
r.make_landmarks(num_landmarks)
seen = [False for row in range(num_landmarks)]
# guess an initial motion
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(N - 1):
# collect sensor measurements in a list, Z
Z = r.sense()
# check off all landmarks that were observed
for i in range(len(Z)):
seen[Z[i][0]] = True
# move
while not r.move(dx, dy):
# if we'd be leaving the robot world, pick instead a new direction
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
# collect/memorize all sensor and motion data
data.append([Z, [dx, dy]])
# we are done when all landmarks were observed; otherwise re-run
complete = (sum(seen) == num_landmarks)
print(' ')
print('Landmarks: ', r.landmarks)
print(r)
return data
def create_robot():
print("-> Create a robot")
print(" ##############")
name = input("Robot name: ")
try:
energy = int(input("Robot energy (range 0-100): "))
while 0 > energy or energy > 100:
print(
"-> ERROR: Energy has to be more than 0 and less than 100")
energy = int(input("Robot energy (range 0-100): "))
except ValueError:
print("-> ERROR: Invalid energy value")
return None
try:
happiness = int(input("Robot happiness (range 0-50): "))
while 0 > happiness or happiness > 50:
print("-> ERROR: Happiness has to be more than 0 and less "
"than 50")
happiness = int(input("Robot happiness (range 0-50): "))
except ValueError:
print("-> ERROR: Invalid happiness value")
return None
try:
procedure = int(input("Robot procedure time (in milliseconds): "))
while procedure < 0:
print("-> ERROR: Procedure time has to be more than 0")
procedure = int(
input("Robot procedure time (in milliseconds): "))
except ValueError:
print("-> ERROR: Invalid procedure time")
return None
owner = input("Robot owner: ")
return Robot(name, energy, happiness, procedure, owner)
def move(self, current, input, delta):
"""
Parameters:
----------
current: np.array(x, y, theta)
current pose
input: np.array(v, omega)
input vector of linear velocity and angular velocity
delta: float
time delta of this tick
Notes:
----------
calculate actual pose with random noise
"""
moved = Robot.move(current, input, delta)
self.actual = np.array([
np.random.normal(moved[0], Agent.actual_xy_sd),
np.random.normal(moved[1], Agent.actual_xy_sd),
np.random.normal(moved[2], Agent.actual_theta_sd)
])
def test_read_instructions_one_robot_noloss(self):
dir_name = self._create_dir()
filepath = f'{dir_name}/allgood'
afile = (f'{filepath}_0', '''5 3\n1 1 E\nRFRFRFRF\n''')
with open(afile[0], 'a') as fl:
fl.write(afile[1])
inst_file_processor = InstructionsFile(afile[0])
inst_file_processor.initialise_instructions()
grid_extents = inst_file_processor.grid_extents
self.assertEqual(grid_extents.coord_x, 5)
self.assertEqual(grid_extents.coord_y, 3)
grid = Grid(grid_extents)
count = 0
for robot_instruction in inst_file_processor.next_instructions():
self.assertEqual(
str(robot_instruction),
'::Start[1,1,E] ::TurnRight ::MoveForward ::TurnRight ::MoveForward ::TurnRight ::MoveForward ::TurnRight ::MoveForward'
)
robot = Robot()
for inst in robot_instruction.instruction_list:
inst.do(grid, robot)
self.assertEqual(str(robot), '1 1 E')
count += 1
self.assertEqual(count, 1)
def test_read_instructions_one_robot_getslost(self):
dir_name = self._create_dir()
filepath = f'{dir_name}/allgood'
afile = (f'{filepath}_1', '''5 3\n3 2 N\nFRRFLLFFRRFLL\n''')
with open(afile[0], 'a') as fl:
fl.write(afile[1])
inst_file_processor = InstructionsFile(afile[0])
inst_file_processor.initialise_instructions()
grid_extents = inst_file_processor.grid_extents
self.assertEqual(grid_extents.coord_x, 5)
self.assertEqual(grid_extents.coord_y, 3)
grid = Grid(grid_extents)
count = 0
for robot_instruction in inst_file_processor.next_instructions():
self.assertEqual(
len(robot_instruction.instruction_list),
14) # 14 instructions (13 moves + 1 start instruction).
self.assertEqual(
str(robot_instruction),
'::Start[3,2,N] ::MoveForward ::TurnRight ::TurnRight ::MoveForward ::TurnLeft ::TurnLeft ::MoveForward ::MoveForward ::TurnRight ::TurnRight ::MoveForward ::TurnLeft ::TurnLeft'
)
robot = Robot()
for inst in robot_instruction.instruction_list[:
8]: # First 8 instructions are good.
inst.do(grid, robot)
robot_instruction.instruction_list[8].do(
grid, robot) # Next instruction leads to robot being lost.
self.assertEqual(str(robot), '3 3 N LOST')
robot_instruction.instruction_list[9].do(grid, robot)
robot_instruction.instruction_list[10].do(grid, robot)
with self.assertRaises(Exception) as exception_context:
robot_instruction.instruction_list[11].do(grid, robot)
for inst in robot_instruction.instruction_list[12:]:
inst.do(grid, robot)
count += 1
self.assertEqual(count, 1)
import sys
from src.grid import Grid
from src.robot import Robot
from src.cleaner_algorithm import dfs
if __name__ == '__main__':
debug = False
# To print all robot movement
if len(sys.argv) > 1 and sys.argv[1].startswith('debug'):
debug = True
grid = Grid([
[1, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 1, 0, 0, 0, 0],
[0, 2, 0, 0, 0, 0, 1, 0],
[2, 2, 2, 0, 2, 2, 2, 2],
[0, 1, 0, 3, 0, 1, 0, 1],
], debug)
robot = Robot(grid, debug)
grid.print_room()
# -------------------
dfs(robot)
# ----------------
grid.print_room()
import threading import time from src.robot import Robot import brickpi import sys #initialize the interface interface = brickpi.Interface() interface.initialize() config = str(sys.argv[1]) Robot = Robot(interface, pid_config_file=config) Robot.approach_object(30, 0) time.sleep(0.5) interface.terminate()
Dis_crate = (1, 2, 3, 4)
Rot_crate = ('LEFT', 'RIGHT', 'RIGHT', 'LEFT')
STAGE_1_NODE_COUNT = {0: 3, 1: 2, 2: 2, 3: 1}
# Stage 2
Pos_cover = 1
Direct = [['RIGHT', 'RIGHT', 'LEFT'], ['LEFT', 'LEFT', 'LEFT'],
['RIGHT', 'LEFT', 'LEFT'], ['LEFT', 'RIGHT', 'LEFT']]
# Blue, yellow, green, red
# Stage 3
Pos_shipyard = 1
# Main
SPEED = 100
COLOURS = {2: "B", 3: "G", 4: "Y", 5: "R"}
current_colour = ""
ROBOT = Robot()
"""Methods in direct relation to path-finding / highest level of logic"""
# Stage 1
def backtrack_to_nearest_node(pos):
ROBOT.follow_black_line_degrees(Dis_crate[pos], SPEED, -1)
back_rotate = 'LEFT'
if Rot_crate[pos] == 'LEFT' or pos == 2:
back_rotate = 'RIGHT'
ROBOT.turn(back_rotate)
if x == 2: # special case for transition between p2 and p3
ROBOT.follow_until_next_node()
ROBOT.turn('AROUND')
else:
class TestRobot(unittest.TestCase):
def setUp(self):
self.compass = Compass("N")
self.field = Field(10,10)
self.robot = Robot(2, 5, self.field, self.compass)
def test_initialize(self):
self.assertEquals(2, self.robot.current_x)
self.assertEquals(5, self.robot.current_y)
self.assertEquals(10, self.robot.field.x_bound)
self.assertEquals(10, self.robot.field.y_bound)
self.assertEquals("N", self.robot.compass.course)
def test_course_by_turns(self):
self.robot.turn("L")
self.assertEquals("W", self.robot.compass.course)
def test_teleport(self):
self.robot.teleport(1,3)
self.assertEquals(1, self.robot.current_x)
self.assertEquals(3, self.robot.current_y)
def test_walk_with_right_steps_for_north(self):
self.robot.walk()
self.assertEquals(2, self.robot.current_x)
self.assertEquals(6, self.robot.current_y)
def test_walk_with_right_steps_for_south(self):
self.robot.compass.course = "S"
self.robot.walk()
self.assertEquals(2, self.robot.current_x)
self.assertEquals(4, self.robot.current_y)
def test_walk_with_right_steps_for_east(self):
self.robot.compass.course = "E"
self.robot.walk()
self.assertEquals(3, self.robot.current_x)
self.assertEquals(5, self.robot.current_y)
def test_walk_with_right_steps_for_west(self):
self.robot.compass.course = "W"
self.robot.walk()
self.assertEquals(1, self.robot.current_x)
self.assertEquals(5, self.robot.current_y)
class Simulation(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.manager = SimulationManager()
self.client_id = self.manager.launchSimulation(gui=True)
p.connect(p.DIRECT)
self.robot = Robot(self.manager, self.client_id)
self.createScene()
self.initUI()
def initUI(self):
self.joint_parameters = list()
for name, joint in self.robot.pepper.joint_dict.items():
if "Finger" not in name and "Thumb" not in name:
self.joint_parameters.append(
(
p.addUserDebugParameter(
name,
joint.getLowerLimit(),
joint.getUpperLimit(),
self.robot.pepper.getAnglesPosition(name)
),
name
)
)
def createScene(self):
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("duck_vhacd.urdf", basePosition=[random.randrange(-3,3),random.randrange(-3,3),0.5], globalScaling=10, )
for i in range(24):
self.createSphere(0.2, random.choice([-5, -4, -3, -2, -1, 1, 2, 4, 5]), random.choice([-5,-4, -3,-2,-1,1,2,3,4,5]), 0.3,
color=(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255)))
def createWall(self, w, h, d, x, y, z):
wall_visual = p.createVisualShape(p.GEOM_BOX, halfExtents=[w, h, d])
wall_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[w, h, d])
wall_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=wall_collision, baseVisualShapeIndex=wall_visual, basePosition=[x, y, z])
def createSphere(self, radius, x, y, z, color=(255, 0, 0)):
sphere_visual = p.createVisualShape(p.GEOM_SPHERE, rgbaColor=[color[0]/255, color[1]/255, color[2]/255, 1], radius=radius)
sphere_collision = p.createCollisionShape(p.GEOM_SPHERE, radius=radius)
sphere_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=sphere_collision,baseVisualShapeIndex=sphere_visual, basePosition=[x, y, z])
def createCube(self, width, x, y, color=(255, 0, 0)):
cube_visual = p.createVisualShape(p.GEOM_BOX, rgbaColor=[color[0]/255, color[1]/255, color[2]/255, 1], halfExtents=[0.5, 0.5, 0.5])
cube_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[width, width, width])
cube_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=cube_collision, baseVisualShapeIndex=cube_visual, basePosition=[x,y,width/2])
def signal_handler(self, signal, frame):
#print("CTRL-C detected")
for t in self.robot.threads:
t.kill()
self.robot.kill()
self.manager.stopSimulation(self.client_id)
sys.exit(0)
def run(self):
self.robot.start()
"""
#Used to follow the robot position but lower performances
while True:
p.resetDebugVisualizerCamera(cameraDistance= p.getDebugVisualizerCamera(self.client_id)[10],
cameraYaw= p.getDebugVisualizerCamera(self.client_id)[8],
cameraPitch= p.getDebugVisualizerCamera(self.client_id)[9],
cameraTargetPosition= [self.robot.pepper.getPosition()[0], self.robot.pepper.getPosition()[1], 0.5])
"""
"""
#Used to find new posture for PiLDIM in case we add more.
for joint_parameter in self.joint_parameters:
self.robot.pepper.setAngles(
joint_parameter[1],
p.readUserDebugParameter(joint_parameter[0]), 1.0
)
"""
parser = parse_args(**tools)
args = parser.parse_args()
if not args.actions:
print("No action selected, exiting...")
sys.exit(1)
log.debug(args)
tool_check()
drrobot = Robot(root_dir=ROOT_DIR,
user_config=USER_CONFIG,
**tools,
dns=getattr(args, 'dns', None),
proxy=getattr(args, 'proxy', None),
domain=getattr(args, 'domain', None),
verbose=getattr(args, 'verbose'),
dbfile=getattr(args, 'dbfile'),
verify=getattr(args, 'verify', None))
if not exists(join_abs(ROOT_DIR, "dbs")):
makedirs(join_abs(ROOT_DIR, "dbs"))
if getattr(args, 'domain', None):
if not exists(join_abs(ROOT_DIR, "output", getattr(args,
'domain'))):
makedirs(join_abs(ROOT_DIR, "output", getattr(args, 'domain')))
if not exists(
join_abs(ROOT_DIR, "output", getattr(args, 'domain'),
import threading
import time
from src.robot import Robot
import brickpi
import sys
#Initialize the interface
interface = brickpi.Interface()
interface.initialize()
config = str(sys.argv[1])
Robot = Robot(interface, pid_config_file=config)
bumpers = [None, None]
speeds = [6, 6]
zeros = [0, 0]
Robot.set_speed(speeds)
#Robot.start_debugging()
while True:
bumpers[0] = Robot.get_bumper("left")
bumpers[1] = Robot.get_bumper("right")
if bumpers[0] and bumpers[1]:
Robot.stop()
Robot.travel_straight(-3)
Robot.rotate_right(30)
Robot.set_speed([-x for x in speeds])
if not bumpers[0] and bumpers[1]:
Robot.stop()
Robot.rotate_left(30)
Robot.set_speed([-x for x in speeds])
import threading
import time
from src.robot import Robot
import brickpi
#Initialize the interface
interface = brickpi.Interface()
interface.initialize()
Robot = Robot(interface, pid_config_file="paper_config.json")
for i in range(4):
time.sleep(1)
Robot.travel_straight(40)
time.sleep(1)
Robot.rotate_left(90)
interface.terminate()
MAP = [[0, 0, "O"], [0, 168, "A"], [84, 168, "B"], [84, 126, "C"],
[84, 210, "D"], [168, 210, "E"], [168, 84, "F"], [210, 84, "G"],
[210, 0, "H"]]
POINTS = [(86, 30), (180, 30), (180, 53), (138, 54), (138, 168), (114, 168),
(114, 84), (86, 84), (86, 30)]
Canvas = Canvas()
Map = Map(Canvas)
Robot = Robot(interface,
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()
import threading
import time
import sys
from src.robot import Robot
import brickpi
#Initialize the interface
interface=brickpi.Interface()
interface.initialize()
interface.startLogging("motor_position_1.log")
Robot = Robot(interface,'carpet_config.json')
Robot.interactive_mode()
interface.stopLogging()
interface.terminate()
def setUp(self):
self.compass = Compass("N")
self.field = Field(10,10)
self.robot = Robot(2, 5, self.field, self.compass)
'''
Test file for the robot.
'''
from src.robot import Robot
import threading
import time
port = '/dev/ttyACM0' # default usb port
#port = '/dev/rfcomm0' # default bluetooth port
ballbot = Robot(port) #make a robot/ballbot
running = True #set the running flag
# Function where the ballbot receives data
def ballbot_update():
while running:
ballbot.receive()
#print(ballbot.x, ballbot.y, ballbot.z, ballbot.roll, ballbot.pitch, ballbot.yaw)
time.sleep(0.05)
# Controller update
dt = 5
def controller_update():
for i in range(0, dt):
print(dt - i)
time.sleep(1)
ballbot.set_attitude_mode() # go to attitude mode
def add_robot(self, genome, name="Robot"):
self.robot_list.append(Robot(self, genome, name))
from src.robot import Robot
from time import sleep
ROBOT = Robot()
print("Started")
ROBOT.follow_until_next_node()
sleep(10)
ROBOT.stop()
print("Stopped")
'''
for i in range(40):
print("L:"+str(ROBOT.left_colour_sensor.reflected_light_intensity))
print("R:"+str(ROBOT.right_colour_sensor.reflected_light_intensity))
'''
'''
print("Left: "+str(ROBOT.left_colour_sensor.reflected_light_intensity))
print("Right: "+str(ROBOT.right_colour_sensor.reflected_light_intensity))
print("Inner: "+str(ROBOT.inner_colour_sensor.reflected_light_intensity))
print("Outer: "+str(ROBOT.outer_colour_sensor.reflected_light_intensity))
'''
'''
for i in range(1):
print("Left: " + str(ROBOT.left_colour_sensor.reflected_light_intensity))
print("Right: " + str(ROBOT.right_colour_sensor.reflected_light_intensity))
sleep(0.2)
'''
