def __init__(self, lm, rm, cs: ColorSensor, odometry: Odometry,
communication: Communication):
# values
self.mode = PilotModes.FOLLOW_LINE
self.vertex = None
self.path = None
self.position = (0, 0, 0)
self.color = None
self.rbd = None
self.touch = False
self.counter = 0
self.status = 'free'
# motors
self.lm = lm
self.rm = rm
# controllers, classes
self.cs = cs
self.odometry = odometry
self.planet = communication.planet
self.communication = communication
self.mixer = MotorMixer(BASE_SPEED, SPEED_MIN, SPEED_MAX)
self.mc = MotorController(K_P, K_I, K_D, I_MAX, SETPOINT)
self.events = EventList()
self.events.add(EventNames.TARGET_REACHED)
EventRegistry.instance().register_event_handler(
EventNames.COLOR, self.set_color)
EventRegistry.instance().register_event_handler(
EventNames.TOUCH, self.set_touch)
EventRegistry.instance().register_event_handler(
EventNames.POSITION, self.set_position)
pass
def __init__(self):
self.dx = 0
self.dy = 0
self.x = 0
self.y = 0
self.heading = 0
self.events = EventList()
self.events.add(EventNames.POSITION)
self.prev_mpos = (0, 0)
pass
def __init__(self, graph, start, target):
super().__init__(graph)
self.start = start
self.target = target
self.unSettledNodes = []
self.predecessors = {}
self.events = EventList()
self.events.add(EventNames.SHORTEST_PATH)
self.start_time = time.time()
pass
class TouchSensor:
def __init__(self):
self.ts1 = ev3.TouchSensor('in2')
self.ts2 = ev3.TouchSensor('in3')
self.events = EventList()
self.events.add(EventNames.TOUCH)
def read_in(self):
value = self.ts1.value() or self.ts2.value()
self.events.set(EventNames.TOUCH, value)
return value
def __init__(self):
super().__init__({}, {})
self.shortest_path = None
self.shortest_path_counter = 0
self.shortest_path_running = False
self.curr_vertex = None
self.curr_path = None
self.target = None
self.mode = PilotModes.EXPLORE
self.paths = {}
self.events = EventList()
self.events.add(EventNames.EXPLORATION_FINISHED)
EventRegistry.instance().register_event_handler(
EventNames.SHORTEST_PATH, self.set_shortest_path)
pass
class ColorSensor:
def __init__(self):
self.cs = ev3.ColorSensor('in1')
self.cs.mode = 'RGB-RAW'
self.events = EventList()
self.events.add(EventNames.COLOR)
def read_in(self):
value = self.cs.bin_data('hhh')
self.events.set(EventNames.COLOR, value)
return value
def get_rgb(self):
return self.cs.bin_data('hhh')
def get_greyscale(self):
value = self.get_rgb()
return (value[0] + value[1] + value[2]) / 3
class Planet(Graph):
def __init__(self):
super().__init__({}, {})
self.shortest_path = None
self.shortest_path_counter = 0
self.shortest_path_running = False
self.curr_vertex = None
self.curr_path = None
self.target = None
self.mode = PilotModes.EXPLORE
self.paths = {}
self.events = EventList()
self.events.add(EventNames.EXPLORATION_FINISHED)
EventRegistry.instance().register_event_handler(
EventNames.SHORTEST_PATH, self.set_shortest_path)
pass
# ---------
# GRAPH MANIPULATION
# ---------
def add_edge(self, start: Path, end: Path, length: float, flag):
edge_to = Edge(start.source, end.source, start.direction,
end.direction, length)
edge_from = Edge(end.source, start.source, end.direction,
start.direction, length)
if flag or (edge_from.id not in self.edges
and edge_to.id not in self.edges):
if start.id in self.paths:
print('deleting path: ', self.paths[start.id])
del self.paths[start.id]
if end.id in self.paths:
print('deleting path: ', self.paths[end.id])
del self.paths[end.id]
self.edges[edge_from.id] = edge_from
self.edges[edge_to.id] = edge_to
return edge_to
else:
return None
def add_vertex(self, position: Tuple[int, int]):
vertex = Vertex(position)
# if vertex.id not in self.vertexes:
self.vertexes[vertex.id] = vertex
return vertex
# else:
# return None
def add_path(self, source: Vertex, direction: Direction):
path = Path(source, direction)
if path.id not in self.paths:
self.paths[path.id] = path
print('new path: ' + str(path))
return path
else:
return None
def vertex_exists(self, position):
for vertex in self.vertexes.values():
if vertex.position == position:
return vertex
return False
# ---------
# SETTER
# ---------
def set_shortest_path(self, sp):
self.shortest_path = sp
self.shortest_path_counter = 0
pass
def set_curr_vertex(self, vertex):
self.curr_vertex = vertex
pass
def set_target_mode(self):
print('TARGET MODE')
self.mode = PilotModes.TARGET
pass
def set_target(self, target):
self.target = target
print('target saved!')
pass
# ---------
# GETTER
# ---------
def get_paths(self):
return self.paths
def get_shortest_path(self, end: Vertex):
self.shortest_path_counter = 0
sp = ShortestPath(self, self.curr_vertex, end)
sp.run()
pass
def get_next_path(self):
print(self.mode, self.mode == PilotModes.TARGET)
if self.mode == PilotModes.EXPLORE:
# depth first
if self.curr_vertex:
paths = list(self.paths.values())
print('paths left:')
for path in paths:
print(path)
for path in self.paths.values():
if self.curr_vertex.equals(path.source):
self.curr_path = path
print('next path: ', self.curr_path)
return self.curr_path
if paths.__len__() > 0:
path = paths[0]
if not self.shortest_path_running:
self.get_shortest_path(path.source)
self.shortest_path_running = True
if self.shortest_path \
and self.shortest_path.__len__() > 0 \
and self.shortest_path_counter < self.shortest_path.__len__():
edge = self.shortest_path[self.shortest_path_counter]
self.shortest_path_counter += 1
self.curr_path = Path(edge.start, edge.start_direction)
print(
'next path: ', self.curr_path,
' sp_counter = ' + str(self.shortest_path_counter))
return self.curr_path
else:
self.events.set(EventNames.EXPLORATION_FINISHED, True)
return False
if self.mode == PilotModes.TARGET:
print('shortest path is:')
if self.shortest_path.__len__() > 0:
for edge in self.shortest_path:
print(edge)
edge = self.shortest_path[self.shortest_path_counter]
self.shortest_path_counter += 1
self.curr_path = Path(edge.start, edge.start_direction)
print('next path: ', self.curr_path,
' sp_counter = ' + str(self.shortest_path_counter))
return self.curr_path
else:
print('shortest path not working or target not reachable!')
return False
else:
return False
def __init__(self):
self.cs = ev3.ColorSensor('in1')
self.cs.mode = 'RGB-RAW'
self.events = EventList()
self.events.add(EventNames.COLOR)
class Odometry:
def __init__(self):
self.dx = 0
self.dy = 0
self.x = 0
self.y = 0
self.heading = 0
self.events = EventList()
self.events.add(EventNames.POSITION)
self.prev_mpos = (0, 0)
pass
def calc_pos1(self, motor_position: Tuple[int, int]):
# print(motor_position)
lec = motor_position[0] - self.prev_mpos[0]
rec = motor_position[1] - self.prev_mpos[1]
displacement = (lec + rec) * ECF / 2
rotation = (lec - rec) * ECF / TRACK
# print(rotation)
self.prev_mpos = motor_position
self.dx = self.dx + displacement * math.sin(self.heading +
rotation / 2)
self.dy = self.dy + displacement * math.cos(self.heading +
rotation / 2)
self.heading = self.heading + rotation
heading = Direction.format(Direction.to_deg(self.heading))
# print(self.dx, self.dy, heading, Direction.str(heading, True))
return self.x, self.y, self.heading
def add_pos(self):
self.x += round(self.dx / 50)
self.y += round(self.dy / 50)
heading = Direction.format(Direction.to_deg(self.heading))
self.events.set(
EventNames.POSITION,
(self.x, self.y, heading, Direction.str(heading, True)))
self.dx = 0
self.dy = 0
# ---------
# SETTER
# ---------
def set_position(self, position):
self.x = position[0]
self.y = position[1]
pass
# ---------
# GETTER
# ---------
def read_in(self, motor_position):
# print('odometry calculated')
return self.calc_pos1(motor_position)
def get_direction(self):
return self.heading
def get_position(self):
return self.x, self.y
def get_vertex(self):
return self.vertex
class Pilot:
# this is the main driving class
# simple or complex maneuvers (follow line, turn, stop, ...) are defined as methods
# PILOT_MODE decides which maneuver is called in the main loop
def __init__(self, lm, rm, cs: ColorSensor, odometry: Odometry,
communication: Communication):
# values
self.mode = PilotModes.FOLLOW_LINE
self.vertex = None
self.path = None
self.position = (0, 0, 0)
self.color = None
self.rbd = None
self.touch = False
self.counter = 0
self.status = 'free'
# motors
self.lm = lm
self.rm = rm
# controllers, classes
self.cs = cs
self.odometry = odometry
self.planet = communication.planet
self.communication = communication
self.mixer = MotorMixer(BASE_SPEED, SPEED_MIN, SPEED_MAX)
self.mc = MotorController(K_P, K_I, K_D, I_MAX, SETPOINT)
self.events = EventList()
self.events.add(EventNames.TARGET_REACHED)
EventRegistry.instance().register_event_handler(
EventNames.COLOR, self.set_color)
EventRegistry.instance().register_event_handler(
EventNames.TOUCH, self.set_touch)
EventRegistry.instance().register_event_handler(
EventNames.POSITION, self.set_position)
pass
def run(self):
if self.mode == PilotModes.FOLLOW_LINE or self.mode == PilotModes.FOLLOW_LINE_ODO:
self.follow_line()
elif self.mode == PilotModes.CHECK_ISC:
self.check_isc()
elif self.mode == PilotModes.CHOOSE_PATH:
self.choose_path()
elif self.mode == PilotModes.BLOCKED:
self.blocked_path()
pass
# ---------
# MANEUVERS
# ---------
def stop_motors(self):
self.set_speed((0, 0))
pass
def follow_line(self):
if self.color is not None:
self.lm.command = 'run-direct'
self.rm.command = 'run-direct'
# calculate rudder from rgb mean
rudder = self.mc.run(self.color)
# divide the rudder speed on the motors
speed = self.mixer.run(rudder)
self.set_speed(speed)
# print(speed)
pass
def blocked_path(self):
self.stop_motors()
self.status = 'blocked'
time.sleep(0.3)
print('path blocked.')
self.lm.run_to_rel_pos(position_sp=-250,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=-250,
speed_sp=200,
stop_action="hold")
self.wait(250, 200)
self.turn_odo(90)
self.lm.run_to_rel_pos(position_sp=60,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=60,
speed_sp=200,
stop_action="hold")
self.wait(60, 200)
self.turn_odo(90)
self.mode = PilotModes.FOLLOW_LINE_ODO
pass
def turn_motor(self, motor, degrees):
# turn by degrees
p_sp = 5.7361 * degrees
if motor is 'lm':
self.lm.run_to_rel_pos(position_sp=p_sp,
speed_sp=200,
stop_action="hold")
if motor is 'rm':
self.rm.run_to_rel_pos(position_sp=p_sp,
speed_sp=200,
stop_action="hold")
return p_sp
def turn(self, degrees):
# turn by degrees
p_sp = 2.88 * degrees
self.lm.run_to_rel_pos(position_sp=-p_sp,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=p_sp,
speed_sp=200,
stop_action="hold")
print('Turning: ' + str(degrees) + ' degrees.')
duration = abs(degrees) / 90 * 1.3
time.sleep(duration)
pass
def turn_odo(self, degrees):
# turn by degrees
p_sp = 2.88 * degrees
self.lm.run_to_rel_pos(position_sp=-p_sp,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=p_sp,
speed_sp=200,
stop_action="hold")
print('Turning: ' + str(degrees) + ' degrees.')
self.wait(p_sp, 200)
# duration = abs(degrees) / 90 * 1.3
# time.sleep(duration)
pass
def check_isc(self):
print('check_isc()')
self.stop_motors()
self.odometry.add_pos()
print(self.position)
self.communication.start()
if self.counter == 0:
self.communication.send_ready()
vertex = self.planet.vertex_exists(
(self.position[0], self.position[1]))
existing_vertex = bool(vertex)
if not existing_vertex:
vertex = self.planet.add_vertex(
(self.position[0], self.position[1]))
if vertex.equals(self.planet.target):
self.events.set(EventNames.TARGET_REACHED, True)
if self.counter != 0 and self.status == 'blocked':
edge = self.planet.add_edge(self.planet.curr_path,
self.planet.curr_path, -1, False)
print('new edge: ', edge)
self.communication.send_edge(edge, 'blocked')
self.planet.set_curr_vertex(vertex)
path = Path(vertex, (self.position[2] + Direction.SOUTH) % 360)
if not existing_vertex:
self.turn(-90)
print('Turning: 362 degrees.')
if self.counter != 0:
self.planet.add_path(self.planet.curr_vertex, path.direction)
if self.counter != 0 and self.status == 'free':
edge = self.planet.add_edge(self.planet.curr_path, path, 0, False)
print('new edge: ', edge)
self.communication.send_edge(edge, 'free')
time.sleep(1)
if not existing_vertex:
self.rm.position = 0
target_pos = self.turn_motor('rm', 362)
eighth = target_pos / 8
while self.rm.position < target_pos - 10:
# print(self.rm.position)
gs = self.cs.get_greyscale()
if gs < 100:
direction = self.position[2]
if target_pos - 7 * eighth <= self.rm.position <= target_pos - 5 * eighth:
direction += Direction.EAST
# print('right path detected: ' + str(direction))
elif target_pos - 5 * eighth <= self.rm.position <= target_pos - 3 * eighth:
direction += Direction.NORTH
# print('straight path detected: ' + str(direction))
elif target_pos - 3 * eighth <= self.rm.position <= target_pos - 1 * eighth:
direction += Direction.WEST
# print('left path detected: ' + str(direction))
else:
continue
direction = direction % 360
# print('path detected: ' + str(direction))
self.planet.add_path(self.planet.curr_vertex, direction)
time.sleep(0.5)
self.turn(90)
pass
if self.status == 'blocked':
self.status = 'free'
self.counter += 1
print('waiting for messages from server!')
time.sleep(2)
self.communication.stop()
self.lm.command = 'reset'
self.rm.command = 'reset'
# self.stop_motors()
# self.lm.position = 0
# self.rm.position = 0
self.odometry.prev_mpos = (0, 0)
self.mode = PilotModes.CHOOSE_PATH
pass
def choose_path(self):
print('choose_path()')
self.stop_motors()
path = self.planet.get_next_path()
if path:
turn_direction = self.position[2] - path.direction
if turn_direction == 270:
turn_direction = -90
if turn_direction == -270:
turn_direction = 90
if turn_direction == -90: # East (relative)
self.lm.run_to_rel_pos(position_sp=60,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=60,
speed_sp=200,
stop_action="hold")
self.wait(60, 200)
self.turn_odo(turn_direction)
self.lm.run_to_rel_pos(position_sp=75,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=75,
speed_sp=200,
stop_action="hold")
self.wait(75, 200)
elif turn_direction == 0: # North (relative)
self.lm.run_to_rel_pos(position_sp=150,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=150,
speed_sp=200,
stop_action="hold")
self.wait(150, 200)
elif turn_direction == 90: # West (relative)
self.lm.run_to_rel_pos(position_sp=110,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=110,
speed_sp=200,
stop_action="hold")
self.wait(110, 200)
self.turn_odo(turn_direction)
self.lm.run_to_rel_pos(position_sp=90,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=90,
speed_sp=200,
stop_action="hold")
self.wait(90, 200)
elif turn_direction == 180 or turn_direction == -180: # South (relative)
self.lm.run_to_rel_pos(position_sp=80,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=80,
speed_sp=200,
stop_action="hold")
self.wait(80, 200)
self.turn_odo(90)
self.lm.run_to_rel_pos(position_sp=50,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=50,
speed_sp=200,
stop_action="hold")
self.wait(50, 200)
self.turn_odo(90)
self.lm.run_to_rel_pos(position_sp=90,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=90,
speed_sp=200,
stop_action="hold")
self.wait(90, 200)
time.sleep(1)
self.mode = PilotModes.FOLLOW_LINE_ODO
pass
def wait(self, target_pos, speed):
i = 5
target_pos = abs(target_pos)
dt = target_pos / speed + 0.5 # dt in s
start = time.time()
while time.time() - start <= dt:
if i == 5:
self.odometry.read_in((self.lm.position, self.rm.position))
i = 0
i += 1
def test(self, value, value2):
print('test()')
self.lm.run_to_rel_pos(position_sp=value,
speed_sp=200,
stop_action="hold")
self.rm.run_to_rel_pos(position_sp=value,
speed_sp=200,
stop_action="hold")
time.sleep(0.5)
self.turn(value2)
pass
# ---------
# SETTER
# ---------
def set_position(self, value):
# print(value)
self.position = value
def set_color(self, value):
self.color = value
self.rbd = value[0] - value[2]
# gs = (value[0] + value[1] + value[2]) / 3
# print('RBD: ' + str(self.rbd) + ' GS: ' + str(gs))
if 90 <= self.rbd: # red patch
print('Patch: red')
self.stop_motors()
self.mode = PilotModes.CHECK_ISC
elif self.rbd <= -65: # blue patch
print('Patch: blue')
self.stop_motors()
self.lm.run_to_rel_pos(position_sp=-35,
speed_sp=180,
stop_action="hold")
time.sleep(0.1)
self.mode = PilotModes.CHECK_ISC
pass
def set_mode(self, mode):
self.mode = mode
pass
def set_touch(self, value):
self.touch = value
if value:
print('touch: ', str(value))
self.mode = PilotModes.BLOCKED
ev3.Sound.beep()
pass
def set_speed(self, speed: Tuple[int, int]):
self.lm.duty_cycle_sp = speed[0]
self.rm.duty_cycle_sp = speed[1]
pass
# ---------
# GETTER
# ---------
def get_position(self):
return self.position
def get_mode(self):
return self.mode
class ShortestPath(PathingAlgorithm.PathingAlgorithm):
def __init__(self, graph, start, target):
super().__init__(graph)
self.start = start
self.target = target
self.unSettledNodes = []
self.predecessors = {}
self.events = EventList()
self.events.add(EventNames.SHORTEST_PATH)
self.start_time = time.time()
pass
def execute(self):
self.distance[self.start.position] = 0
self.unSettledNodes.append(self.start)
while self.unSettledNodes.__len__() > 0:
node = self.get_minimum(self.unSettledNodes)
self.settledNodes.append(node)
self.unSettledNodes.remove(node)
self.find_minimal_distances(node)
if time.time() - self.start_time >= 10:
break
return self
def find_minimal_distances(self, node):
adjacent_nodes = self.get_neighbors(node)
for target in adjacent_nodes:
if (self.get_shortest_distance(target) >
self.get_shortest_distance(node) +
self.get_distance(node, target)):
self.distance[target.position] = self.get_shortest_distance(
node) + self.get_distance(node, target)
self.predecessors[target.position] = node
self.unSettledNodes.append(target)
pass
def get_path(self):
vertexes = []
step = self.target
# check if a vertexes exists
if self.predecessors.get(step.position) is None:
return None
vertexes.append(step)
while self.predecessors.get(step.position):
step = self.predecessors.get(step.position)
vertexes.append(step)
vertexes.append(self.start)
vertexes.pop()
vertexes.reverse()
path = []
length = vertexes.__len__() - 2
if length > 0:
for i in range(vertexes.__len__() - 1):
if i >= 0:
for e in self.edges.values():
if vertexes[i].equals(
e.start) and vertexes[i + 1].equals(e.end):
path.append(e)
return path
def run(self):
print('ShortestPath Thread started!')
path = self.execute().get_path()
self.events.set(EventNames.SHORTEST_PATH, path)
print('ShortestPath Thread finished!')
pass
def __init__(self):
self.ts1 = ev3.TouchSensor('in2')
self.ts2 = ev3.TouchSensor('in3')
self.events = EventList()
self.events.add(EventNames.TOUCH)