def setupDashboardControls(): # Only need to set up dashbard drive controls once global modeChooser if modeChooser == None: SmartDashboard.putBoolean("Quick Turn", False) SmartDashboard.putBoolean("Square Inputs", True) SmartDashboard.putNumber("Fixed Left", 0.4) SmartDashboard.putNumber("Fixed Right", 0.4) SmartDashboard.putNumber("Rotation Gain", 0.5) SmartDashboard.putNumber("Slow Gain", 0.5) mc = SendableChooser() mc.addDefault("Arcade", kModeArcade) mc.addOption("Tank", kModeTank) mc.addOption("Curvature", kModeCurvature) mc.addDefault("Indexed Arcade", kModeIndexedArcade) mc.addDefault("Indexed Tank", kModeIndexedTank) mc.addOption("Fixed", kModeFixed) SmartDashboard.putData("Drive Mode", mc) modeChooser = mc
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)