def __init__(self, known=False, file_name=False):
self.width = 2.0
self.height = 1.2
# set up walls, putting top left point first
self.walls = []
self.walls.append(E160_wall([-0.5, 0.5, -0.5, -0.5], "vertical"))
self.walls.append(E160_wall([0.5, 0.5, 0.5, -0.5], "vertical"))
self.walls.append(E160_wall([-0.5, 0.5, 0.5, 0.5], "horizontal"))
# self.walls.append(E160_wall([0.5, -0.5, 1, -0.5],"horizontal"))
# self.walls.append(E160_wall([-0.5, -0.5, 0.5, -1],"horizontal"))
# self.walls.append(E160_wall([-0.5, -0.5, 0.0, -1.0],"vertical"))
# create vars for hardware vs simulation
self.robot_mode = "SIMULATION MODE" #"SIMULATION MODE" or "HARDWARE MODE"
self.control_mode = "MANUAL CONTROL MODE"
# setup xbee communication
if (self.robot_mode == "HARDWARE MODE"):
self.serial_port = serial.Serial('/dev/ttyUSB0', 9600)
print " Setting up serial port"
try:
self.xbee = XBee(self.serial_port)
except:
print("Couldn't find the serial port")
# Setup the robots
self.num_robots = 1
self.robots = []
for i in range(0, self.num_robots):
# TODO: assign different address to each bot
r = E160_robot(self, '\x00\x0C', i, known, file_name)
self.robots.append(r)
def get_walls_leap(self, robot_id, particle_states):
all_walls = [i for i in self.walls] # Copy the walls.
for i, r in enumerate(self.robots):
if i != robot_id:
e = E160_state()
e.set_from_tuple(particle_states[i])
radius = r.radius
new_wall_r = E160_wall([e.x+radius, e.y+radius, e.x+radius,
e.y-radius], "vertical")
new_wall_l = E160_wall([e.x-radius, e.y+radius, e.x-radius,
e.y-radius], "vertical")
new_wall_t = E160_wall([e.x-radius, e.y+radius, e.x+radius,
e.y+radius], "horizontal")
new_wall_b = E160_wall([e.x-radius, e.y-radius, e.x+radius,
e.y-radius], "horizontal")
#print("new_wall_r {}".format(new_wall_r))
all_walls += [new_wall_r, new_wall_l, new_wall_t, new_wall_b]
return all_walls
def __init__(self, maze=[], corners=[], pixel_pos=(), file_name=False):
self.width = 2.5
self.height = 2.0
self.cell_width = 0.45
self.cell_height = 0.4
self.top_left_corner = corners[0]
self.bottom_right_corner = corners[3]
self.pixel_width = self.bottom_right_corner[0] - self.top_left_corner[0]
self.pixel_height = self.bottom_right_corner[1] - self.top_left_corner[
1]
self.maze = E160_maze(maze)
# set up walls, putting top left point first
self.walls = []
for start, end, slope in self.maze.walls:
self.walls.append(
E160_wall([
(-self.width / 2 + 0.2) - self.top_left_corner[0] *
(self.width / self.pixel_width) +
start[0] * self.cell_width,
(self.height / 2 - 0.2) + self.top_left_corner[1] *
(self.width / self.pixel_width) +
start[1] * self.cell_height,
(-self.width / 2 + 0.2) - self.top_left_corner[0] *
(self.width / self.pixel_width) + end[0] * self.cell_width,
(self.height / 2 - 0.2) + self.top_left_corner[1] *
(self.width / self.pixel_width) + end[1] * self.cell_height
], slope))
# create vars for hardware vs simulation
self.robot_mode = "HARDWARE MODE" #"SIMULATION MODE" or "HARDWARE MODE"
self.control_mode = "AUTONOMOUS CONTROL MODE"
self.track_mode = "PATH MODE" #"POINT MODE" or "PATH MODE"
# setup xbee communication
if (self.robot_mode == "HARDWARE MODE"):
self.serial_port = serial.Serial('/dev/ttyUSB0', 9600)
print " Setting up serial port"
try:
self.xbee = XBee(self.serial_port)
except:
print("Couldn't find the serial port")
# Setup the robots
self.num_robots = 1
self.robots = []
for i in range(0, self.num_robots):
botActualPos = self.get_pos_from_pixel_pos(pixel_pos)
# TODO: assign different address to each bot
r = E160_robot(self, '\x00\x0C', i, botActualPos, file_name)
self.robots.append(r)
def __init__(self):
self.width = 2.0
self.height = 2.2
self.cell_edge_length = 0.05
self.robot_radius = 0.147/2
# set up walls, putting top left point first
self.walls = []
self.walls.append(E160_wall([-0.5, 0.1, -0.5, -0.1],"vertical"))
#self.walls.append(E160_wall([0.5, 0.5, 0.5, -0.5],"vertical"))
self.walls.append(E160_wall([-0.1, 0.5, 0.1, 0.5],"horizontal"))
self.walls.append(E160_wall([0.0, -0.6, 0.0, -.7],"vertical"))
self.walls.append(E160_wall([0.6, -1.0, 1.0, -1.0],"horizontal"))
#self.walls.append(E160_wall([0.7, 0.5, .8, 0.5],"horizontal"))
#self.walls.append(E160_wall([1.0, -0.5, 1.0, -1.0],"vertical"))
#self.walls.append(E160_wall([1.0, -1.0, 1.0, -1.5],"vertical"))
#set up the AntGrid
self.grid = self.AntGrid(self.width, self.height, self.walls, self.cell_edge_length, self.robot_radius)
self.grid.initializeGrid()
# create vars for hardware vs simulation
self.robot_mode = "SIMULATION MODE"#"SIMULATION MODE" or "HARDWARE MODE"
self.control_mode = "AUTONOMOUS CONTROL MODE" #"MANUAL CONTROL MODE"
# setup xbee communication
if (self.robot_mode == "HARDWARE MODE"):
self.serial_port = serial.Serial('/dev/tty.usbserial-DN01IWND', 9600)
print" Setting up serial port"
try:
self.xbee = XBee(self.serial_port)
except:
print "Couldn't find the serial port"
# Setup the robots
self.num_robots = 1
self.robots = []
for i in range (0,self.num_robots):
# TODO: assign different address to each bot
r = E160_robot(self, '\x00\x0C', i)
self.robots.append(r)
def add_wall(self):
self.environment.control_mode = "AUTONOMOUS CONTROL MODE"
# update sliders on gui
self.forward_control.set(0)
self.rotate_control.set(0)
self.last_forward_control = 0
self.last_rotate_control = 0
self.R = 0
self.L = 0
# add wall
self.environment.walls.append(E160_wall([0.5, 0.1, 0.5, -0.1],"vertical"))
for r in self.environment.robots:
r.replan_path = True
r.robot.motion_plan()
x_des = float(self.x_des_entry.get())
y_des = float(self.y_des_entry.get())
theta_des = float(self.theta_des_entry.get())
r.state_des.set_state(x_des,y_des,theta_des)
r.point_tracked = False
def updateObstacles(self):
self.walls.append(E160_wall([0.5, 0.1, 0.5, -0.1], "vertical"))
def __init__(self, mode="SIMULATION MODE"):
#self.width = 2.0
#self.height = 1.2
self.width = 6.0
self.height = 3.0
# set up walls, putting top left point first
self.walls = []
#self.walls.append(E160_wall([-0.5, 0.3, -0.5, -0.3], "vertical"))
#self.walls.append(E160_wall([1, 0.8, 1, -0.3], "vertical"))
self.walls.append(E160_wall([0, 1, 15, 1], "horizontal"))
self.walls.append(E160_wall([0, -1, 15, -1], "horizontal"))
#self.walls.append(E160_wall([-1, 0.8, -1, -0.3], "vertical"))
# create vars for hardware vs simulation
# "SIMULATION MODE" or "HARDWARE MODE"
self.robot_mode = mode
self.using_leapfrog = True
self.control_mode = "MANUAL CONTROL MODE"
# setup xbee communication
if self.robot_mode == "HARDWARE MODE":
self.serial = serial.Serial('COM9', 9600)
print("Setting up serial port")
try:
self.xbee = XBee(self.serial)
print("Found Computer XBee")
except:
print("Couldn't find the serial port")
#serial_port2 = serial.Serial('COM5', 9600)
#print(" Setting up serial port")
#try:
# self.xbee2 = XBee(serial_port2)
#except:
# print("Couldn't find the serial port")
# Setup the robots
self.file_name = 'Log/{}_All_Bots' '_' \
+ datetime.datetime.now() \
.replace(microsecond=0) \
.strftime('%y-%m-%d %H.%M.%S') + '.txt'
self.file_name = self.file_name.format(self.robot_mode)
self.num_robots = 2
if self.num_robots == 1:
self.robot_pos = [(0, 0, 0)]
elif self.num_robots == 2:
self.robot_pos = [(0.5, 0, 0), (0, 0, 0)]
self.robots = []
self.state_odo = [E160_state() for _ in range(self.num_robots)]
addresses = ['\x00\x0C', '\x00\x01']
for i in range(self.num_robots):
# TODO: assign different address to each bot
r = E160_robot(self, addresses[i], i)
self.robots.append(r)
self.state_odo[i].set_from_tuple(self.robot_pos[i])
# Pair the two robots up
if self.num_robots == 2:
self.robots[0].other_pair_id = 1
self.robots[1].other_pair_id = 0
# Store the measurements of all robots here.
self.range_meas = [[0] for _ in self.robots]
self.encoder_meas = [[0, 0] for _ in self.robots]
self.last_encoder_meas = [[0, 0] for _ in self.robots]
self.bearing_from_other = [0 for _ in self.robots]
self.pf = E160_PF.E160_PF(self, self.robots)
self.make_header(self.num_robots)
def __init__(self):
self.width = 3.5
self.height = 3.0
# create vars for hardware vs simulation
self.robot_mode = "HARDWARE MODE" # "SIMULATION MODE" or "HARDWARE MODE"
self.control_mode = "LINE FOLLOW MODE"
self.walls = []
if self.robot_mode == 'SIMULATION MODE':
# set up walls, putting top left point first
lines = [[7,10,7,0], [0,0,10,0], [2,2,2,-2], [2,1,7,1],\
[7,6,15,6], [7,4,15,4], [10,2,10,0], [10,1,15,1] ,[15,6,15,-2]]
for wall in lines:
# Scale the walls down to reasonable sizes
wall[:] = [round(x/5, 1) for x in wall]
# Shift it into the frame
wall[0] = wall[0] - 1.5
wall[2] = wall[2] - 1.5
wall[1] = wall[1] - 1
wall[3] = wall[3] - 1
if wall[0] == wall[2]:
self.walls.append(E160_wall(wall, "vertical"))
elif wall[1] == wall[3]:
self.walls.append(E160_wall(wall, "horizontal"))
else:
print('Discarded wall:', wall)
# self.walls.append(E160_wall([0.5, 0.5, 0.5, -0.5], "vertical"))
# self.walls.append(E160_wall([-0.5, 0.5, 0.5, 0.5], "horizontal"))
# self.walls.append(E160_wall([0.5, -0.5, 1.0, -0.5], "horizontal"))
# self.walls.append(E160_wall([0.0, -0.5, 0.0, -1.0], "vertical"))
# setup xbee communication
if (self.robot_mode == "HARDWARE MODE"):
self.serial_port = serial.Serial(
'/dev/tty.usbserial-DN02Z1BF', 9600)
print(" Setting up serial port")
try:
self.xbee = XBee(self.serial_port)
except:
print("Couldn't find the serial port")
# Setup the robots
self.num_robots = 1
self.robots = []
for i in range(0, self.num_robots):
# TODO: assign different address to each bot
r = E160_robot(self, '\x00\x0C', i)
self.robots.append(r)
if self.robot_mode == "HARDWARE MODE":
input("Press enter to begin calibration.")
print('starting calibration')
r.calibrate()
def __init__(self):
# self.width = 4.5
# self.height = 2.5
self.width = 9
self.height = 5
self.INtoCM = 0.0254
# set up walls, putting top left point first
self.walls = []
# self.walls.append(E160_wall([-1, 0, 0, 0]))
# self.walls.append(E160_wall([-1, 0.5, 0, 0.5]))
# self.walls.append(E160_wall([-1, -0.5, 0, -0.5]))
# self.walls.append(E160_wall([1, -1, 1, 1]))
# outer walls
# self.walls.append(E160_wall([-1, -1.25, -1, 1.25]))
# self.walls.append(E160_wall([-1, -1.25, 1, -1]))
# self.walls.append(E160_wall([-1, 1.25, 1, 1]))
### self.walls.append(E160_wall([-0.5, -0.5, -0.5, 0.5],"vertical"))
### self.walls.append(E160_wall([0.5, -0.5, 0.5, 0.5],"vertical"))
### self.walls.append(E160_wall([-0.5, 0.5, 0.5, 0.5],"horizontal"))
## South Maze
# outer walls
# self.walls.append(E160_wall([-2, -2, -2, 2]))
# self.walls.append(E160_wall([2, -2, 2, 2]))
# self.walls.append(E160_wall([-2, -2, 2, -2]))
# self.walls.append(E160_wall([-2, 2, 2, 2]))
# #
# self.walls.append(E160_wall([-2, 2 - 44*self.INtoCM, -2 + 14.5*self.INtoCM, 2 - 44*self.INtoCM]))
# self.walls.append(E160_wall([-2 + 22*self.INtoCM, 2, -2 + 22*self.INtoCM, 2 - 28*self.INtoCM]))
# self.walls.append(E160_wall([-2 + (6.5+22)*self.INtoCM, 2, -2 + (6.5+22)*self.INtoCM, 2 - 17*self.INtoCM]))
# self.walls.append(E160_wall([-2 + (3.5+22)*self.INtoCM, 2-17*self.INtoCM, -2 + (3.5+22)*self.INtoCM, 2 - 28*self.INtoCM]))
## self.walls.append(E160_wall([-2 + (3.5+22)*self.INtoCM, 2-17*self.INtoCM, -2 + (3.5+22)*self.INtoCM, 2 - 28*self.INtoCM]))
# self.walls.append(E160_wall([-2, 2 - 44*self.INtoCM - 28*self.INtoCM, 2, 2 - 44*self.INtoCM - 28*self.INtoCM]))
## corner test
# self.walls.append(E160_wall([1, -1, 1, 1]))
# self.walls.append(E160_wall([-1, -1, 1, -1]))
# self.walls.append(E160_wall([-1, -1+48*self.INtoCM, 1, -1+48*self.INtoCM]))
## Parsons corner map [x1, y1, x2, y2] (Next to Lab/Room 2391)
wall1a = [0, 0, 0, 0.94]
wall1b = [0, 1.85, 0, 4.01]
door1a = [-0.12, 0.94, -0.12, 1.85]
door1b = [-0.12, 0.94, 0, 0.94]
door1c = [-0.12, 1.85, 0, 1.85]
wall2 = [0, 0, 4, 0]
wall3 = [1.75, 1.8, 1.75, 4]
wall4 = [1.75, 1.8, 1.75 + 0.87, 1.8]
wall5 = [1.75 + 0.87, 1.8, 1.75 + 0.87, 1.8 - 0.38]
wall6 = [1.75 + 0.87, 1.8 - 0.38, 4, 1.8 - 0.38]
self.walls.append(E160_wall(wall1a))
self.walls.append(E160_wall(wall1b))
self.walls.append(E160_wall(wall2))
self.walls.append(E160_wall(wall3))
self.walls.append(E160_wall(wall4))
self.walls.append(E160_wall(wall5))
self.walls.append(E160_wall(wall6))
self.walls.append(E160_wall(door1a))
self.walls.append(E160_wall(door1b))
self.walls.append(E160_wall(door1c))
# added obstacles
wall7 = [1.75, 2, 0.75, 2]
wall8 = [3, 1.8 - 0.38, 3, 0.5]
wall9 = [1.5, 0, 1.5, 1]
self.walls.append(E160_wall(wall7))
self.walls.append(E160_wall(wall8))
self.walls.append(E160_wall(wall9))
# create vars for hardware vs simulation
self.robot_mode = "SIMULATION MODE" #"SIMULATION MODE" or "HARDWARE MODE"
self.control_mode = "MANUAL CONTROL MODE"
if self.robot_mode == "SIMULATION MODE":
endcap1 = [-0.01, 4, 1.75, 4]
endcap2 = [4, 0, 4, 1.8 - 0.38]
self.walls.append(E160_wall(endcap1))
self.walls.append(E160_wall(endcap2))
# setup xbee communication
if (self.robot_mode == "HARDWARE MODE"):
try:
self.serial_port = serial.Serial('/dev/tty.usbserial-DN02Z6QQ',
9600)
self.xbee = XBee(self.serial_port)
print("Setting up serial port")
except:
print("Serial port/XBee not found")
# Setup the robots
self.num_robots = 1
self.robots = []
for i in range(0, self.num_robots):
# TODO: assign different address to each bot
r = E160_robot(self, '\x00\x0C', i)
self.robots.append(r)