def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.running = {}
self.omni_driving = True
self.omni_drive = omni_drive
self.drive_motors = DriveMotors(self)
self.oi = OI(self)
self.chassis = Chassis(self)
self.auto_tasks = move_forward_auto # [[list, of, tasks, to_go, through, sequentially], [and, this, list, will, run, in, parallel]
self.current_auto_tasks = []
self.vision_array = Array("d", [0.0, 0.0, 0.0, 0.0])
self.vision_terminate_event = Event()
self.vision = Vision(self.vision_array, self.vision_terminate_event)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.running = {}
self.omni_driving = True
self.omni_drive = omni_drive
self.drive_motors = DriveMotors(self)
self.oi = OI(self)
self.chassis = Chassis(self)
self.auto_tasks = move_forward_auto # [[list, of, tasks, to_go, through, sequentially], [and, this, list, will, run, in, parallel]
self.current_auto_tasks = []
self.vision_array = Array("d", [0.0, 0.0, 0.0, 0.0])
self.vision_terminate_event = Event()
self.vision = Vision(self.vision_array, self.vision_terminate_event)
class StrongholdRobot(wpilib.IterativeRobot):
logger = logging.getLogger("robot")
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.running = {}
self.omni_driving = True
self.omni_drive = omni_drive
self.drive_motors = DriveMotors(self)
self.oi = OI(self)
self.chassis = Chassis(self)
self.auto_tasks = move_forward_auto # [[list, of, tasks, to_go, through, sequentially], [and, this, list, will, run, in, parallel]
self.current_auto_tasks = []
self.vision_array = Array("d", [0.0, 0.0, 0.0, 0.0])
self.vision_terminate_event = Event()
self.vision = Vision(self.vision_array, self.vision_terminate_event)
def disabledInit(self):
self.vision_terminate_event.clear()
def disabledPeriodic(self):
"""This function is called periodically when disabled."""
self.running = {}
self.vision_terminate_event.clear()
def autonomousInit(self):
self.running = {}
self.current_auto_tasks = self.auto_tasks
self.vision_terminate_event.set()
try:
self.vision.start()
except:
pass
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
# clear empty task sequences
for i in range(len(self.auto_tasks)):
if len(self.auto_tasks[i]) == 0:
self.auto_tasks[i].pop()
for command_sequence in self.current_auto_tasks:
if command_sequence[0] not in self.running:
ifunc = command_sequence[0](self).__next__
self.running[command_sequence[0]] = ifunc
done = []
for key, ifunc in self.running.items():
try:
ifunc()
except StopIteration:
done.append(key)
for key in done:
self.running.pop(key)
# remove the done task from our list of auto commands
for command_sequence in self.current_auto_tasks:
if key == command_sequence[0]:
command_sequence.pop(0)
def teleopInit(self):
self.running = {}
self.omni_driving = True
self.vision_terminate_event.set()
try:
self.vision.start()
except:
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
for button, task in taskmap.items():
#if the button for the task is pressed and the task is not already running
if self.oi.joystick.getRawButton(button) and task not in self.running:
ifunc = task(self).__next__
self.running[task] = ifunc
if self.omni_driving and self.omni_drive not in self.running:
ifunc = self.omni_drive(self).__next__
self.running[self.omni_drive] = ifunc
done = []
for key, ifunc in self.running.items():
try:
ifunc()
except StopIteration:
done.append(key)
for key in done:
self.running.pop(key)
#self.logger.info("Teleop periodic vision: " + str(self.vision_array[0]))
def testPeriodic(self):
"""This function is called periodically during test mode."""
wpilib.LiveWindow.run()
class StrongholdRobot(wpilib.IterativeRobot):
logger = logging.getLogger("robot")
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.running = {}
self.omni_driving = True
self.omni_drive = omni_drive
self.drive_motors = DriveMotors(self)
self.oi = OI(self)
self.chassis = Chassis(self)
self.auto_tasks = move_forward_auto # [[list, of, tasks, to_go, through, sequentially], [and, this, list, will, run, in, parallel]
self.current_auto_tasks = []
self.vision_array = Array("d", [0.0, 0.0, 0.0, 0.0])
self.vision_terminate_event = Event()
self.vision = Vision(self.vision_array, self.vision_terminate_event)
def disabledInit(self):
self.vision_terminate_event.clear()
def disabledPeriodic(self):
"""This function is called periodically when disabled."""
self.running = {}
self.vision_terminate_event.clear()
def autonomousInit(self):
self.running = {}
self.current_auto_tasks = self.auto_tasks
self.vision_terminate_event.set()
try:
self.vision.start()
except:
pass
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
# clear empty task sequences
for i in range(len(self.auto_tasks)):
if len(self.auto_tasks[i]) == 0:
self.auto_tasks[i].pop()
for command_sequence in self.current_auto_tasks:
if command_sequence[0] not in self.running:
ifunc = command_sequence[0](self).__next__
self.running[command_sequence[0]] = ifunc
done = []
for key, ifunc in self.running.items():
try:
ifunc()
except StopIteration:
done.append(key)
for key in done:
self.running.pop(key)
# remove the done task from our list of auto commands
for command_sequence in self.current_auto_tasks:
if key == command_sequence[0]:
command_sequence.pop(0)
def teleopInit(self):
self.running = {}
self.omni_driving = True
self.vision_terminate_event.set()
try:
self.vision.start()
except:
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
for button, task in taskmap.items():
#if the button for the task is pressed and the task is not already running
if self.oi.joystick.getRawButton(
button) and task not in self.running:
ifunc = task(self).__next__
self.running[task] = ifunc
if self.omni_driving and self.omni_drive not in self.running:
ifunc = self.omni_drive(self).__next__
self.running[self.omni_drive] = ifunc
done = []
for key, ifunc in self.running.items():
try:
ifunc()
except StopIteration:
done.append(key)
for key in done:
self.running.pop(key)
#self.logger.info("Teleop periodic vision: " + str(self.vision_array[0]))
def testPeriodic(self):
"""This function is called periodically during test mode."""
wpilib.LiveWindow.run()