def teleopInit(self):
self.loops = 0
self.timer.reset()
self.timer.start()
Scheduler.getInstance().removeAll()
Scheduler.getInstance().enable()
def autonomousInit(self):
Scheduler.getInstance().removeAll()
# Set up encoders
# Loop counter to stop/start auto?
# Get Driverstation data from field
data = wpilib.DriverStation.getInstance().getGameSpecificMessage()
def disabledPeriodic(self):
Scheduler.getInstance().run()
#Tanklift data
SmartDashboard.putBoolean("Front IR", subsystems.drivelift.frontIR.state)
SmartDashboard.putBoolean("Back IR", subsystems.drivelift.backIR.state)
#Elevator Data
SmartDashboard.putNumber("Elevator Ticks", subsystems.elevator.elevatorEncoder.get())
SmartDashboard.putNumber("Elevator Height", subsystems.elevator.elevatorEncoder.getDistance())
SmartDashboard.putNumber("Limit Switch", subsystems.elevator.btmLimitSwitch.get())
#Game Objective States
SmartDashboard.putBoolean("Ramp Tog", subsystems.ramp.rampToggle)
SmartDashboard.putBoolean("Hatch Tog", subsystems.hatchgrab.hatchToggle)
SmartDashboard.putBoolean("Suction Extend", subsystems.hatchgrab.hatchExtendToggle)
SmartDashboard.putBoolean("Cargo Tog", subsystems.cargograb.cargoToggle)
SmartDashboard.putNumber("R Servo Angle", subsystems.cargograb.rightServo.getAngle())
SmartDashboard.putNumber("L Servo Angle", subsystems.cargograb.leftServo.getAngle())
SmartDashboard.putBoolean("Relay State", subsystems.hatchgrab.hatchGrabExtendSol.get())
#Driveline data
SmartDashboard.putNumber("Left Speed", subsystems.driveline.leftSpdCtrl.get())
SmartDashboard.putNumber("Right Speed", subsystems.driveline.rightSpdCtrl.get())
SmartDashboard.putNumber("Right Sensors", subsystems.driveline.rSensor.getVoltage())
SmartDashboard.putNumber("Left Sensors", subsystems.driveline.lSensor.getVoltage())
SmartDashboard.putNumber("Sensor Dif", subsystems.driveline.lineSensorCompare)
def operatorControl(self):
joystick = self.oi.getStick()
while self.isOperatorControl() and self.isEnabled():
self.log()
Scheduler.getInstance().run()
wpilib.Timer.delay(.005)
def whileActive(self, command) -> None:
"""Constantly starts the given command while the button is held.
:meth:`Command.start` will be called repeatedly while the trigger is
active, and will be canceled when the trigger becomes inactive.
:param command: the command to start
"""
command = self._checkInput(command)
def execute() -> None:
pressed = self.grab()
if pressed:
command.start()
elif execute.pressedLast and not pressed:
command.cancel()
execute.pressedLast = pressed
execute.pressedLast = self.grab()
from wpilib.command import Scheduler
Scheduler.getInstance().addButton(execute)
def autonomous(self):
"""Auton code."""
#Logging loop
while self.isAutonomous() and self.isEnabled():
Scheduler.getInstance().run()
self.log()
wpilib.Timer.delay(.005) #don't burn up the cpu
def autonomousPeriodic(self):
if self.timer.hasPeriodPassed(2):
print("Autonomous Run")
if (self.oneShot == False):
Scheduler.getInstance().addCommand(ParaCommandGr())
self.oneShot = True
Scheduler.getInstance().run()
def autonomous(self):
"""Autonomous commands go in here."""
#self.drivetrain.drive.setSafetyEnabled(False) - disable the safety mode on the drivetrain
self.PlayMacroCommand(self, "macro.csv")
while self.isAutonomous() and self.isEnabled():
Scheduler.getInstance().run()
self.log()
wpilib.Timer.delay(.005) #don't burn up the cpu
def teleopPeriodic(self):
Scheduler.getInstance().run()
speed = self.right_joy.getY()
rotation = self.left_joy.getX()
self.drivetrain.drive.arcadeDrive(speed, rotation, True)
self.loops += 1
if self.timer.hasPeriodPassed(1):
self.logger.info("%d loops / second", self.loops)
self.loops = 0
def operatorControl(self):
"""Teleop code."""
#Make the joystick work for driving:
joystick = self.oi.getStick()
#Logging loop
while self.isOperatorControl() and self.isEnabled():
self.log()
Scheduler.getInstance().run()
wpilib.Timer.delay(.005) #don't burn up the cpu
def operatorControl(self):
"""Teleop stuff goes in here"""
#self.drivetrain.drive.setSafetyEnabled(True) - enables safety things for manual control
joystick = self.oi.getStick()
while self.isOperatorControl() and self.isEnabled():
self.log()
Scheduler.getInstance().run()
wpilib.Timer.delay(.005) #don't burn up the cpu
def execute(self):
#toggle state
self.getRobot().ramp.set(not self.getRobot().ramp.get())
#change the color of the blinkins to a "chase" pattern
Scheduler.getInstance().add(commands.lights.SetColor("bluechase"))
if self.getRobot().ramp.get():
self.getRobot().blinkin.setDefaultCommand(commands.lights.SetColor("bluechase"))
else:
self.getRobot().blinkin.setDefaultCommand(commands.lights.SetColor("defaultgradient"))
def teleopPeriodic(self):
# Before, button functions were executed here. Now scheduler will do that
Scheduler.getInstance().run()
# Keeping track of TimedRobot loops through code
self.loops += 1
if self.timer.hasPeriodPassed(1):
self.logger.info("%d loops / second", self.loops)
self.loops = 0
def operatorControl(self):
"""Teleop code."""
print("Teleop activated")
self.simpleAutonCommand.cancel()
self.shooterAutonCommand.cancel()
#Make the joystick work for driving:
joystick = self.oi.getStick()
#Logging loop
while self.isOperatorControl() and self.isEnabled():
self.log()
Scheduler.getInstance().run()
wpilib.Timer.delay(.05) #don't burn up the cpu
def operatorControl(self):
'''Runs the motors with Mecanum drive.'''
self.robotDrive.setSafetyEnabled(True)
while self.isOperatorControl() and self.isEnabled():
Scheduler.getInstance().run()
self.robotDrive.mecanumDrive_Cartesian(self.oi.stick.getX(),
self.oi.stick.getY(),
self.oi.stick.getZ(), 0);
wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# make this true to ignore joystick errors
self.debug = False
self.enabled_time = 0
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.navigation = Navigation(self)
self.pneumatics = Pneumatics(self)
self.peripherals = Peripherals(self)
#wpilib.SmartDashboard.putData(self.drivetrain)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
wpilib.SmartDashboard.putData(Scheduler.getInstance())
# instantiate the command used for the autonomous period
self.autonomousCommand = None
def run_command(cmd):
cmd.start()
scheduler = Scheduler.getInstance()
for x in range(0, 2000):
scheduler.run()
if not cmd.isRunning():
return True
return False
def operatorControl(self):
"""Runs the 'bot with the fancy joystick and an awesome gamepad THAT IS AWESOME."""
#Cancel the autons - that could go badly otherwise.
self.ThreeToteAutonomousCommand.cancel()
self.CanAutonomousCommand.cancel()
self.DriveAutonomousCommand.cancel()
self.ToteAutonomousCommand.cancel()
self.PlayMacroCommand.cancel()
self.CanNToteAutoCommand.cancel()
#More stuff.
self.drivetrain.drive.setSafetyEnabled(True)
joystick = self.oi.getJoystickLeft()
while self.isOperatorControl() and self.isEnabled():
self.log()
Scheduler.getInstance().run()
wpilib.Timer.delay(.005) # don't burn up the cpu
def run_command(cmd):
cmd.start()
scheduler = Scheduler.getInstance()
for x in range(0, 2000):
scheduler.run()
if not cmd.isRunning():
return True
return False
def autonomous(self):
"""Auton code."""
print("Autonomous mode activated")
try:
if self.oi.smart_dashboard.getBoolean("Play Macro"):
self.simpleAutonCommand.start()
elif self.oi.smart_dashboard.getBoolean("Shoot"):
self.shooterAutonCommand.start()
else:
print("No macro selected, waiting for teleop...")
except KeyError:
pass
#Logging loop
while self.isAutonomous() and self.isEnabled():
Scheduler.getInstance().run()
self.log()
wpilib.Timer.delay(.05) #don't burn up the cpu
def cancelWhenActive(self, command) -> None:
"""Cancels a command when the trigger becomes active.
:param command: the command to cancel
"""
command = self._checkInput(command)
def execute() -> None:
pressed = self.grab()
if not execute.pressedLast and pressed:
command.cancel()
execute.pressedLast = pressed
execute.pressedLast = self.grab()
from wpilib.command import Scheduler
Scheduler.getInstance().addButton(execute)
def whenActive(self, command) -> None:
"""Starts the given command whenever the trigger just becomes active.
:param command: the command to start
"""
command = self._checkInput(command)
def execute() -> None:
pressed = self.grab()
if not execute.pressedLast and pressed:
command.start()
execute.pressedLast = pressed
execute.pressedLast = self.grab()
from wpilib.command import Scheduler
Scheduler.getInstance().addButton(execute)
return self
def whenInactive(self, command) -> None:
"""Starts the command when the trigger becomes inactive.
:param command: the command to start
"""
command = self._checkInput(command)
def execute() -> None:
pressed = self.grab()
if execute.pressedLast and not pressed:
command.start()
execute.pressedLast = pressed
execute.pressedLast = self.grab()
from wpilib.command import Scheduler
Scheduler.getInstance().addButton(execute)
def autonomousPeriodic(self):
"""
Periodic code for autonomous mode should go here. This method will be called every 20ms.
"""
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
if self.scheduleAutonomous:
self.scheduleAutonomous = False
logger.info("Game Data: %s" % (self.gameData))
logger.info("Cross Field Enable: %s" % (self.crossFieldEnable))
logger.info("Scoring Element %s" % (self.scoringElement))
logger.info("Starting Position %s" % (self.startingPosition))
self.autonMiddleStartLeftSwitch.start()
#self.autonLeftStartLeftScale.start()
#self.autonForward.start()
Scheduler.getInstance().run()
def autonomous(self):
"""Woo, auton code w/ 3 modes. Needs to be tested."""
#I'll probably change these out to different macro commands.
self.drivetrain.drive.setSafetyEnabled(False)
try:
if self.oi.smart_dashboard.getBoolean("3 Tote Auto"):
self.ThreeToteAutonomousCommand.start()
print("3 Tote Auto started")
elif self.oi.smart_dashboard.getBoolean("Can Auto"):
self.CanAutonomousCommand.start()
print("Can Auto started")
elif self.oi.smart_dashboard.getBoolean("Can/Tote Auto"):
self.CanNToteAutoCommand.start()
print("Can and Tote Auto started")
elif self.oi.smart_dashboard.getBoolean("Tote Auto"):
self.ToteAutonomousCommand.start()
print("Tote Auto started")
elif self.oi.smart_dashboard.getBoolean("Play Macro"):
self.PlayMacroCommand.start()
print("Macro replay started")
elif self.oi.smart_dashboard.getBoolean("Drive Auto"):
self.DriveAutonomousCommand.start()
print("Drive Auto started")
else:
pass
except KeyError:
self.PlayMacroCommand.start()
#pass
while self.isAutonomous() and self.isEnabled():
Scheduler.getInstance().run()
self.log()
wpilib.Timer.delay(.005) # don't burn up the cpu
def start(self):
scheduler = Scheduler.getInstance()
for subsystem in scheduler.subsystems:
subsystem.initDefaultCommand()
try:
while True:
scheduler.run()
except KeyboardInterrupt:
self.shutdown()
def toggleWhenActive(self, command) -> None:
"""Toggles a command when the trigger becomes active.
:param command: the command to toggle
"""
command = self._checkInput(command)
def execute() -> None:
pressed = self.grab()
if not execute.pressedLast and pressed:
if command.isRunning():
command.cancel()
else:
command.start()
execute.pressedLast = pressed
execute.pressedLast = self.grab()
from wpilib.command import Scheduler
Scheduler.getInstance().addButton(execute)
def autonomousInit(self):
ahrs = AHRS.create_spi()
#ahrs.getYaw()
ahrs.getAngle()
oneGroup = CommandGroup(
) # CHANGED (parentheses are needed to create a CommandGroup object)
# CHANGED:
oneGroup.addSequential(
DriveForwardCommand(self.leftFront, self.rightFront, self.leftBack,
self.rightBack, 1.0, 1.0)) # speed, seconds
oneGroup.addSequential(
RotateCommand(self.leftFront, self.leftBack, self.rightFront,
self.rightBack, self.ahrs, math.radians(45)))
scheduler = Scheduler.getInstance()
scheduler.add(oneGroup)
def disabledPeriodic(self):
self.disabled_mode.periodic_update()
Scheduler.getInstance().run()
def teleopPeriodic(self):
Scheduler.getInstance().run()
self.datalogger.log_data()
def autonomousPeriodic(self):
Scheduler.getInstance().run()
self.datalogger.log_data()
def teleopPeriodic(self):
Scheduler.getInstance().run()
self.logger.info(self.getPeriod())
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
self.auto_mode.periodic_update()
Scheduler.getInstance().run()
def teleopPeriodic(self):
Scheduler.getInstance().run()
self.updateDashboard()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous"""
Scheduler.getInstance().run()
self.log()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous. Uses the
Scheduler to run enqueued commands."""
Scheduler.getInstance().run()
def autonomous(self):
while self.isAutonomous() and self.isEnabled():
Scheduler.getInstance().run()
self.log()
wpilib.Timer.delay(.005)
def teleopPeriodic(self):
"""Runs continually during teleop. Leverages the Scheduler class to
run enqueued commands.
"""
Scheduler.getInstance().run()
def teleopPeriodic(self):
'''Called every 20ms in teleoperated mode'''
Scheduler.getInstance().run()
def testPeriodic(self):
"""This function is called periodically during test mode."""
self.test_mode.periodic_update()
wpilib.LiveWindow.run()
Scheduler.getInstance().run()
def teleopPeriodic(self):
'''This function is called periodically during operator control'''
Scheduler.getInstance().run()
self.log()
def autonomousPeriodic(self):
'''This function is called periodically during autonomous'''
Scheduler.getInstance().run()
self.log()
def startCompetition(self):
"""Initalizes the scheduler before starting robotInit()"""
self.scheduler = Scheduler.getInstance()
super().startCompetition()
def testPeriodic(self):
Scheduler.getInstance().run()
if self.timer.hasPeriodPassed(2):
print("Test Run")
def disabledPeriodic(self):
Scheduler.getInstance().run()
self.updateDashboard()
def teleopInit():
Scheduler.getInstance().removeAll()
def autonomousPeriodic(self):
Scheduler.getInstance().run()
self.updateDashboard()
def autonomousPeriodic():
Scheduler.getInstance().run()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
Scheduler.getInstance().run()
self.log()
def teleopPeriodic():
Scheduler.getInstance().run()
def autonomousPeriodic(self):
Scheduler.getInstance().addCommand(Command_Bad_Auto(self))
Scheduler.getInstance().run()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
self.teleop_mode.periodic_update()
Scheduler.getInstance().run()
def teleopPeriodic(self):
Scheduler.getInstance().run()
# add later
if self.timer.hasPeriodPassed(2):
print("Teleop method")
def teleopPeriodic(self):
Scheduler.getInstance().run()