class MyRobot(CommandBasedRobot):
dashboard = True
frequency = 20
period = 1 / frequency
def __init__(self):
super().__init__(self.period)
def robotInit(self):
'''
This is a good place to set up your subsystems and anything else that
you will need to access later.
'''
wpilib.CameraServer.launch()
self.lastAuto = False
self.lastAlign = False
self.lastStraightAlign = False
self.teleop = False
Command.getRobot = lambda x=0: self
# Load system preferences prior to constructing
# subsystems
map.loadPreferences()
# Construct subsystems prior to constructing commands
self.limelight = Limelight.Limelight(self) #not a subsystem
self.hatch = HatchMech.HatchMech(self)
self.hatch.initialize()
#self.cargo = CargoMech0.CargoMech()
self.cargo = CargoMech.CargoMech() #not a subsystem
self.cargo.initialize()
self.climber = Climber.Climber() #not a subsystem
self.climber.initialize(self)
self.drive = Drive.Drive(self)
self.compressor = wpilib.Compressor(0)
self.timer = wpilib.Timer()
self.timer.start()
self.watch = wpilib.Watchdog(150, None)
'''
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
'''
[self.joystick0, self.joystick1, self.xbox] = oi.commands()
self.updateDashboardInit()
self.DriveStraight = DriveStraight()
self.DriveStraightSide = DriveStraightSide()
self.LeftCargo = LeftCargo()
self.RightCargo = RightCargo()
self.CenterCargo = CenterCargo()
self.SetSpeedDT = SetSpeedDT()
# Set up auton chooser
self.autonChooser = SendableChooser()
#self.autonChooser.setDefaultOption("Drive Straight", "DriveStraight")
self.autonChooser.setDefaultOption("Level 1 Center", "Level1Center")
self.autonChooser.addOption("Left Cargo", "LeftCargo")
self.autonChooser.addOption("Right Cargo", "RightCargo")
self.autonChooser.addOption("Do Nothing", "DoNothing")
self.autonChooser.addOption("Level 1 Center", "Level1Center")
self.autonChooser.addOption("Drive Straight Side", "DriveStraightSide")
SmartDashboard.putData("Auto mode", self.autonChooser)
def robotPeriodic(self):
self.limelight.readLimelightData()
SmartDashboard.putBoolean("teleop", self.teleop)
if (self.dashboard): self.updateDashboardPeriodic()
def autonomousInit(self):
super().autonomousInit()
self.drive.zero()
self.timer.reset()
self.timer.start()
self.autoSelector(self.autonChooser.getSelected())
def autonomousPeriodic(self):
super().autonomousPeriodic()
#driver takes control of drivetrain
deadband = 0.1
if (abs(self.joystick0.getRawAxis(map.drive)) > abs(deadband)):
self.SetSpeedDT.start()
if (abs(self.joystick1.getRawAxis(map.drive)) > abs(deadband)):
self.SetSpeedDT.start()
self.cargo.periodic()
def teleopInit(self):
super().teleopInit()
self.disabledInit()
self.teleop = True
def teleopPeriodic(self):
self.climber.periodic()
self.cargo.periodic()
self.hatch.periodic()
super().teleopPeriodic()
def updateDashboardInit(self):
#self.drive.dashboardInit()
#self.hatch.dashboardInit()
self.cargo.dashboardInit()
#self.climber.dashboardInit()
#self.limelight.dashboardInit()
#sequences.dashboardInit()
#autonomous.dashboardInit()
#pass
def updateDashboardPeriodic(self):
#SmartDashboard.putNumber("Timer", self.timer.get())
self.drive.dashboardPeriodic()
#self.hatch.dashboardPeriodic()
self.cargo.dashboardPeriodic()
#self.climber.dashboardPeriodic()
#self.limelight.dashboardPeriodic()
#sequences.dashboardPeriodic()
#autonomous.dashboardPeriodic()
#pass
def disabledInit(self):
self.scheduler.removeAll()
self.drive.disable()
self.hatch.disable()
self.cargo.disable()
self.climber.disable()
def disabledPeriodic(self):
self.disabledInit()
def driverLeftButton(self, id):
""" Return a button off of the left driver joystick that we want to map a command to. """
return wpilib.buttons.JoystickButton(self.joystick0, id)
def driverRightButton(self, id):
""" Return a button off of the right driver joystick that we want to map a command to. """
return wpilib.buttons.JoystickButton(self.joystick1, id)
def operatorButton(self, id):
""" Return a button off of the operator gamepad that we want to map a command to. """
return wpilib.buttons.JoystickButton(self.xbox, id)
def operatorAxis(self, id):
""" Return a Joystick off of the operator gamepad that we want to map a command to. """
#id is axis channel for taking value of axis
return self.xbox.getRawAxis(id)
#wpilib.joystick.setAxisChannel(self.xbox, id)
def readOperatorButton(self, id):
""" Return button value """
return self.xbox.getRawButton(id)
def readDriverRightButton(self, id):
""" Return button value from right joystick """
return self.joystick1.getRawButton(id)
def readDriverLeftButton(self, id):
""" Return button value from left joystick """
return self.joystick0.getRawButton(id)
def autoSelector(self, auto):
if auto == "DriveStraight":
self.DriveStraight.start()
elif auto == "DoNothing":
self.disabledInit()
elif auto == "Level1Center":
self.CenterCargo.start()
elif auto == "DriverControl":
self.driverControl()
elif auto == "DriveStraightSide":
self.DriveStraightSide.start()
elif auto == "RightCargo":
self.RightCargo.start()
elif auto == "LeftCargo":
self.LeftCargo.start()
else:
self.disabledInit()
def driverControl(self):
self.SetSpeedDT.start()
class OI():
debug: bool = True
autonChooser: SendableChooser = None
driver: Joystick = None
def __init__(self):
self.driver: Joystick = Joystick(0)
self.setupDriverCommands()
self.setupDashboardCommands()
@staticmethod
def initializeNumber(label: str, defaultValue: float) -> float:
value: float = SmartDashboard.getNumber(label, defaultValue)
SmartDashboard.putNumber(label, value)
return value
@staticmethod
def initializeBoolean(label: str, defaultValue: bool) -> bool:
value: bool = SmartDashboard.getBoolean(label, defaultValue)
SmartDashboard.putBoolean(label, value)
return value
def createDriverButton(self, rawId: int) -> JoystickButton:
""" Returns a button object that you can attach event handlers (commands)
to execute when the driver presses, holds or releases the button on the gamepad. """
return JoystickButton(self.driver, rawId)
def readDriverAxis(self, rawAxisId: int) -> float:
""" Reads value of an analog axis on the driver game pad [-1.0, +1.0]. """
return self.driver.getRawAxis(rawAxisId)
def readDriverButton(self, rawButtonId: int) -> bool:
""" Returns true if driver is currently pressing the button specified. """
return self.driver.getRawButton(rawButtonId)
def setupDriverCommands(self):
flipButton = self.createDriverButton(5)
flipButton.whenPressed(DriveHuman.createFlipFrontCommand())
def getSelectedAuton(self) -> Command:
return self.autonChooser.getSelected()
def setupDashboardCommands(self):
# Set up auton chooser
self.autonChooser = SendableChooser()
self.autonChooser.setDefaultOption(
"Do Nothing", wpilib.command.WaitCommand(3.0, "Do Nothing"))
self.autonChooser.addOption("Drive Forward", DriveTickTimed(1.0, 1.0))
self.autonChooser.addOption("Drive Backward",
DriveTickTimed(-1.0, -1.0))
self.autonChooser.addOption("Rotate Right", DriveTickTimed(1.0, -1.0))
self.autonChooser.addOption("Rotate Left", DriveTickTimed(-1.0, 1.0))
SmartDashboard.putData("Auto mode", self.autonChooser)
# Drive controls
DriveHuman.setupDashboardControls()
SmartDashboard.putData("Brake Control",
DriveHuman.createBrakeModeToggleCommand())
SmartDashboard.putData("Flip Front",
DriveHuman.createFlipFrontCommand())
# Set up utility controls
SmartDashboard.putData("Measure", Measure())
# Climber settings
SmartDashboard.putData("Full Auto Climb", ClimbUp())
if self.debug:
SmartDashboard.putData("Extend Both Legs", ExtendBothLegs())
SmartDashboard.putData("Drive to Front Sensor",
DriveToFrontSensor())
SmartDashboard.putData("Retract Front Legs", RetractFrontLegs())
SmartDashboard.putData("Drive to Back Sensor", DriveToBackSensor())
SmartDashboard.putData("Retract Back Legs", RetractBackLegs())
# Debug tools (if enabled)
if self.debug:
SmartDashboard.putData("CPU Load Test", LoadTest())
SmartDashboard.putData("Drive Subsystem", subsystems.drive)
dd = subsystems.drive.getDifferentialDrive()
if dd != None:
SmartDashboard.putData("DifferentialDrive", dd)
