Example #1
0
    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
Example #2
0
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()
Example #3
0
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)