def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', quick=False, is_attacker=False):
"""
Entry point for the SDP system.
Params:
[int] video_port port number for the camera
[string] comm_port port number for the arduino
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] our_side the side we're on - 'left' or 'right'
# """
self.controller = Controller(comm_port)
if not quick:
print("Waiting 10 seconds for serial to initialise")
time.sleep(10)
# Kick once to ensure we are in the correct position
self.controller.update(CommandDict.kick())
self.pitch = pitch
# Set up the vision system
self.vision = VisionWrapper(pitch, color, our_side, video_port)
# Set up the planner
self.planner = Planner(our_side, pitch, attacker=is_attacker)
# Set up GUI
self.GUI = GUI(calibration=self.vision.calibration, pitch=pitch, launch=self)
self.color = color
self.side = our_side
self.control_loop()
record = True
else:
record = False
if (args.opt1 == 'vision' or args.opt2 == 'vision'
or args.opt3 == 'vision'):
extras = True
else:
extras = False
if (args.opt1 == 'verbose' or args.opt2 == 'verbose'
or args.opt3 == 'verbose'):
verbose = "y"
else:
verbose = "n"
# start vision system in background thread
vis = VisionWrapper(pitch=pitch_number, record=record, extras=extras)
t = Thread(target=vis.run)
t.daemon = True
t.start()
inp = ""
strategy = Strategy()
comms = Comms()
while True:
try:
comms_active = comms.start()
if comms_active:
while inp != "t" or inp != "s" or inp != "r":
inp = raw_input("Tests, start or reset? (t/s/r) : ")
if inp == "s":
strategy.start4(comms, verbose)
class Main:
"""
Primary source of robot control. Ties vision and planning together.
"""
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', quick=False, is_attacker=False):
"""
Entry point for the SDP system.
Params:
[int] video_port port number for the camera
[string] comm_port port number for the arduino
[int] pitch 0 - main pitch, 1 - secondary pitch
[string] our_side the side we're on - 'left' or 'right'
# """
self.controller = Controller(comm_port)
if not quick:
print("Waiting 10 seconds for serial to initialise")
time.sleep(10)
# Kick once to ensure we are in the correct position
self.controller.update(CommandDict.kick())
self.pitch = pitch
# Set up the vision system
self.vision = VisionWrapper(pitch, color, our_side, video_port)
# Set up the planner
self.planner = Planner(our_side, pitch, attacker=is_attacker)
# Set up GUI
self.GUI = GUI(calibration=self.vision.calibration, pitch=pitch, launch=self)
self.color = color
self.side = our_side
self.control_loop()
def control_loop(self):
#TODO change this description ESC doesn't work
"""
The main loop for the control system. Runs until ESC is pressed.
Takes a frame from the camera; processes it, gets the world state;
gets the actions for the robots to perform; passes it to the robot
controllers before finally updating the GUI.
"""
counter = 1L
timer = time.clock()
try:
key = 255
while key != 27: # the ESC key
# update the vision system with the next frame
self.vision.update()
pre_options = self.vision.preprocessing.options
# Find appropriate action
command = self.planner.update(self.vision.model_positions)
self.controller.update(command)
# Information about states
regular_positions = self.vision.regular_positions
model_positions = self.vision.model_positions
defenderState = (str(self.planner.current_plan), "0")
# Use 'y', 'b', 'r' to change color.
key = waitKey(delay=2) & 0xFF # Returns 255 if no keypress detected
gui_actions = []
fps = float(counter) / (time.clock() - timer)
# Draw vision content and actions
self.GUI.draw(
self.vision.frame, model_positions, gui_actions, regular_positions, fps, None,
defenderState, None, None, False,
our_color=self.color, our_side=self.side, key=key, preprocess=pre_options)
counter += 1
except Exception as e:
print(e.message)
traceback.print_exc(file=sys.stdout)
finally:
tools.save_colors(self.pitch, self.vision.calibration)
