class OI:
def __init__(self, robot):
self.joy1 = wpilib.Joystick(constants.DRIVER_STATION_JOYSTICK_0)
self.joy1_btn_a = JoystickButton(self.joy1, 1)
self.joy1_btn_b = JoystickButton(self.joy1, 2)
self.joy1_btn_x = JoystickButton(self.joy1, 3)
self.joy1_btn_y = JoystickButton(self.joy1, 4)
self.joy1_btn_lb = JoystickButton(self.joy1, 5)
self.joy1_btn_rb = JoystickButton(self.joy1, 6)
self.joy2 = wpilib.Joystick(constants.DRIVER_STATION_JOYSTICK_1)
self.joy2_btn_a = JoystickButton(self.joy2, 1)
self.joy2_btn_b = JoystickButton(self.joy2, 2)
self.joy2_btn_x = JoystickButton(self.joy2, 3)
self.joy2_btn_y = JoystickButton(self.joy2, 4)
self.joy2_btn_select = JoystickButton(self.joy2, 7)
self.joy2_btn_start = JoystickButton(self.joy2, 8)
#self.joy1_btn_a.whileActive(TestCommand(robot))
'''
self.joy1_btn_a.whenPressed(ExtendShoulder(robot))
self.joy1_btn_a.whenReleased(StopShoulder(robot))
self.joy1_btn_b.whenPressed(RetractShoulder(robot))
self.joy1_btn_b.whenReleased(StopShoulder(robot))
self.joy1_btn_x.whenPressed(ExtendElbow(robot))
self.joy1_btn_x.whenReleased(StopElbow(robot))
self.joy1_btn_y.whenPressed(RetractElbow(robot))
self.joy1_btn_y.whenReleased(StopElbow(robot))
self.joy2_btn_a.whenPressed(ExtendWrist(robot))
self.joy2_btn_a.whenReleased(StopWrist(robot))
self.joy2_btn_b.whenPressed(RetractWrist(robot))
self.joy2_btn_b.whenReleased(StopWrist(robot))
'''
self.joy2_btn_x.whenPressed(PickupIntake(robot))
self.joy2_btn_x.whenReleased(StopIntake(robot))
self.joy2_btn_y.whenPressed(EjectIntake(robot))
self.joy2_btn_y.whenReleased(StopIntake(robot))
self.joy1_btn_x.whenPressed(ExtendLift(robot))
self.joy2_btn_a.whenPressed(ArmGoToPickup(robot))
self.joy2_btn_a.whenReleased(ArmStop(robot))
self.joy2_btn_b.whenPressed(ArmGoToScore(robot))
self.joy2_btn_b.whenReleased(ArmStop(robot))
self.joy2_btn_start.whenPressed(ArmGoToTower(robot))
self.joy2_btn_start.whenReleased(ArmStop(robot))
def get_joystick():
"""
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
"""
joystick = Joystick(0)
joystick2 = Joystick(1)
# TODO lots of commands to map here
# TODO how are we going to count taps for position?
# trigger = JoystickButton(joystick=joystick, buttonNumber=5)
# trigger.whenPressed(gear_shift.gear_shift(True))
# trigger.whenReleased(gear_shift.gear_shift(False))
# rtrigger = JoystickButton(joystick=joystick, buttonNumber=6)
# rtrigger.whenPressed(set_arm_pos.SetArmPosition(1024.0))
# rtrigger.whenReleased(set_arm_pos.SetArmPosition(0.0))
buttonB = JoystickButton(joystick=joystick, buttonNumber=2)
buttonB.whenPressed(hatch_punch(True))
buttonB.whenReleased(hatch_punch(False))
buttonX = JoystickButton(joystick=joystick, buttonNumber=3)
buttonX.whenPressed(SetElvPay(-3000, 470))
buttonA = JoystickButton(joystick=joystick, buttonNumber=1)
buttonA.whenPressed(SetElvPay(-3000, 2250))
buttonL1 = JoystickButton(joystick=joystick, buttonNumber=5)
buttonL1.whenPressed(BallIn())
buttonL1.whenReleased(BallStop())
buttonR1 = JoystickButton(joystick=joystick, buttonNumber=6)
buttonR1.whenPressed(BallOut())
buttonR1.whenReleased(BallStop())
#button12 = JoystickButton(joystick=joystick2, buttonNumber=12)
#button12.whenPressed(SetElvPay(0, 0))
#buttonB = JoystickButton(joystick=joystick2, buttonNumber=2)
#buttonB.whenPressed(SetElvSpeed(0.5))
#buttonB.whenReleased(SetElvSpeed(0.0))
#buttonA = JoystickButton(joystick=joystick2, buttonNumber=1)
#buttonA.whenPressed(SetElvSpeed(-0.5))
#buttonA.whenReleased(SetElvSpeed(0.0))
#high hatch
buttonL1 = JoystickButton(joystick=joystick2, buttonNumber=5)
buttonL1.whenPressed(SetElvPay(-25000, 470))
#mid hatch
buttonX = JoystickButton(joystick=joystick2, buttonNumber=3)
buttonX.whenPressed(SetElvPay(-17000, 470))
#stow
buttonA = JoystickButton(joystick=joystick2, buttonNumber=1)
buttonA.whenPressed(SetElvPay(0, 0))
#high ball
buttonR1 = JoystickButton(joystick=joystick2, buttonNumber=6)
buttonR1.whenPressed(SetElvPay(-25000, 1100))
#mid ball
buttonY = JoystickButton(joystick=joystick2, buttonNumber=4)
buttonY.whenPressed(SetElvPay(-17000, 1100))
#low ball
buttonB = JoystickButton(joystick=joystick2, buttonNumber=2)
buttonB.whenPressed(SetElvPay(-3000, 1100))
#elevator up
buttonLS = JoystickButton(joystick=joystick2, buttonNumber=9)
buttonLS.whenPressed(SetElvSpeed(-0.7))
buttonLS.whenReleased(SetElvSpeed(0.0))
#all stop
buttonRS = JoystickButton(joystick=joystick2, buttonNumber=10)
buttonRS.whenPressed(SetElvSpeed(0.0))
buttonRS.whenPressed(SetPaySpeed(0.0))
#elbow up
buttonBack = JoystickButton(joystick=joystick2, buttonNumber=7)
buttonBack.whenPressed(SetPaySpeed(-0.3))
buttonBack.whenReleased(SetPaySpeed(0.0))
#elbow down
buttonStart = JoystickButton(joystick=joystick2, buttonNumber=8)
buttonStart.whenPressed(SetPaySpeed(0.3))
buttonStart.whenReleased(SetPaySpeed(0.0))
return joystick
class PyBot(CommandBasedRobot):
'''
The CommandBasedRobot base class implements almost everything you need for
a working robot program. All you need to do is set up the subsystems and
commands. You do not need to override the "periodic" functions, as they
will automatically call the scheduler. You may override the "init" functions
if you want to do anything special when the mode changes.
'''
def robotInit(self):
'''
Subsystem, Command, OperatorInterface Instantiation
IN THAT ORDER
'''
Command.getRobot = lambda x=0: self
#self.motor = SingleMotor.SingleMotor(2, self.logger)
self.drivetrain = DriveTrain(self.logger)
self.elevator = Elevator(self.logger)
self.arm = Arm(self.logger)
self.scooper = Scooper(self.logger)
self.startCommand = DeployArm()
self.joystick = Joystick(robotmap.STK_port)
self.halfPowerModeButton = JoystickButton(self.joystick,
robotmap.STK_halfPowerMode)
# self.automaticMode = JoystickButton(self.joystick, robotmap.STK_automode)
# self.automaticMode.whenPressed(DockRobot())
# self.automaticMode.whenReleased(CancelSubsystem(Cancel.DRIVETRAIN))
self.buttonBoard = Joystick(robotmap.BB_port)
self.armUpButton = JoystickButton(self.buttonBoard, robotmap.BB_ArmUp)
self.armDownButton = JoystickButton(self.buttonBoard,
robotmap.BB_ArmDown)
self.stowArmButton = JoystickButton(self.buttonBoard,
robotmap.BB_StowArm)
self.elevatorUpButton = JoystickButton(self.buttonBoard,
robotmap.BB_ElevatorUp)
self.elevatorDownButton = JoystickButton(self.buttonBoard,
robotmap.BB_ElevatorDown)
self.ingestButton = JoystickButton(self.buttonBoard,
robotmap.BB_ingest)
self.expelButton = JoystickButton(self.buttonBoard, robotmap.BB_expel)
self.deployArmButton = JoystickButton(self.buttonBoard,
robotmap.BB_DeployArm)
self.midHatchButton = JoystickButton(self.buttonBoard,
robotmap.BB_MidHatch)
self.lowCargoButton = JoystickButton(self.buttonBoard,
robotmap.BB_LowCargo)
self.midCargoButton = JoystickButton(self.buttonBoard,
robotmap.BB_MidCargo)
self.armUpButton.whenPressed(MoveArmUp())
self.armDownButton.whenPressed(MoveArmDown())
self.armUpButton.whenReleased(CancelSubsystem(Cancel.ARM))
self.armDownButton.whenReleased(CancelSubsystem(Cancel.ARM))
self.stowArmButton.whenPressed(StowArm())
#self.deployArmButton.whenPressed(DeployArm())
self.elevatorUpButton.whenPressed(MoveElevatorUp())
self.elevatorDownButton.whenPressed(MoveElevatorDown())
self.elevatorUpButton.whenReleased(CancelSubsystem(Cancel.ELEVATOR))
self.elevatorDownButton.whenReleased(CancelSubsystem(Cancel.ELEVATOR))
self.ingestButton.whenPressed(Ingest())
self.expelButton.whenPressed(Expel())
# self.lowHatchButton.whenPressed(PositionLowHatch())
# self.midHatchButton.whenPressed(PositionMidHatch())
# self.lowCargoButton.whenPressed(PositionLowCargo())
# self.midCargoButton.whenPressed(PositionMidCargo())
self._smartDashboard = NetworkTables.getTable('SmartDashboard')
self._smartDashboard.putString("ArmLimit", "InGame")
class MyRobot(commandbased.CommandBasedRobot):
def robotInit(self):
'''This is where the robot code starts.'''
team3200.getRobot = lambda x=0:self
self.map = team3200.robotMap.RobotMap()
self.networkTableInit()
self.dtSub = team3200.subsystems.driveTrain.DriveTrainSub()
self.jGreatDrive = joystickDrive.JoystickDrive(-.8)
self.jDrive = self.jGreatDrive
self.driveInit(self.jDrive)
self.liftHold = True
self.liftSub = team3200.subsystems.lifter.LifterSub()
self.pistonSub = team3200.subsystems.lifter.PlatePiston()
self.driveController = wpilib.XboxController(self.map.controllerMap.driverController['controllerId'])
self.auxController = wpilib.XboxController(self.map.controllerMap.auxController['controllerId'])
self.controllerInit()
self.healthMonitor = team3200.subsystems.healthMonitor.HealthMonitor()
def networkTableInit(self):
'''This sets up the network tables and adds a variable called sensitivity'''
NetworkTables.initialize(server = 'roborio-3200-frc.local')
self.liveWindowTable = NetworkTables.getTable('Custom')
for k, v in self.map.networkTableMap.networkTableValues.items():
#K for key, V for value
self.liveWindowTable.putNumber(k, v)
def controllerInit(self):
self.driveController = wpilib.XboxController(self.map.controllerMap.driverController['controllerId'])
self.auxController = wpilib.XboxController(self.map.controllerMap.auxController['controllerId'])
self.driveControllerMap = self.map.controllerMap.driverController
self.auxControllerMap = self.map.controllerMap.auxController
#Spacing added for readability
'''Buttons for the Drive Controller'''
self.lightButton = JoystickButton(self.driveController, self.driveControllerMap['ledToggle'])
self.lightButton.whenPressed(Lights())
self.leftButton = JoystickButton(self.driveController, self.driveControllerMap['leftButton'])
self.leftButton.whenPressed(joystickDrive.GearDown(self))
self.rightButton = JoystickButton(self.driveController, self.driveControllerMap['rightButton'])
self.rightButton.whileHeld(joystickDrive.GearUp(self))
self.alignButton = JoystickButton(self.driveController, self.driveControllerMap['alignButton'])
self.alignButton.whenPressed(AlignButton(self.dtSub))
self.straightButton = JoystickButton(self.driveController, self.driveControllerMap['straightButton'])
self.straightButton.whileHeld(DriveStraight(self.dtSub))
'''Buttons for the Auxiliary Controller'''
self.raiseButton = JoystickButton(self.auxController, self.auxControllerMap['RaiseButton'])
if self.liftHold:
self.raiseButton.whileHeld(lifterControl.RaiseButton(self.liftSub))
self.raiseButton.whenReleased(lifterControl.StopButton(self.liftSub))
else:
self.raiseButton.whenPressed(lifterControl.RaiseButton(self.liftSub))
self.lowerButton = JoystickButton(self.auxController, self.auxControllerMap['LowerButton'])
if self.liftHold:
self.lowerButton.whileHeld(lifterControl.LowerButton(self.liftSub))
self.lowerButton.whenReleased(lifterControl.StopButton(self.liftSub))
else:
self.lowerButton.whenPressed(lifterControl.LowerButton(self.liftSub))
self.pistonButton = JoystickButton(self.auxController, self.auxControllerMap['PistonButton'])
self.pistonButton.whenPressed(lifterControl.PistonButton(self.pistonSub))
self.rollerIO = JoystickButton(self.auxController, self.auxControllerMap['RollerIO'])
self.rollerIO.whenPressed(lifterControl.ForwardRoller(self.liftSub))
self.rollerIO.whenReleased(lifterControl.StopRoller(self.liftSub))
self.rollerToggle = JoystickButton(self.auxController, self.auxControllerMap['RollerToggle'])
self.rollerToggle.whenPressed(lifterControl.ReverseRoller(self.liftSub))
self.rollerToggle.whenReleased(lifterControl.StopRoller(self.liftSub))
def driveInit(self, jDrive):
self.dtSub = team3200.subsystems.driveTrain.DriveTrainSub()
self.jDrive = jDrive
sensName = "ControllerSensitivity"
if sensName in self.map.networkTableMap.networkTableValues:
sensEntry = self.liveWindowTable.getEntry(sensName)
self.jDrive = team3200.commands.joystickDrive.JoystickDrive(sensEntry)
self.dtSub.setDefaultCommand(self.jDrive)
class OI(object):
def __init__(self):
# Create Joysticks
self.stick = Joystick(0)
self.gamepad = Joystick(1)
# Create Buttons
self.trigger = JoystickButton(self.stick, 1)
self.button2 = JoystickButton(self.stick, 2)
self.button3 = JoystickButton(self.stick, 3)
self.button4 = JoystickButton(self.stick, 4)
self.button5 = JoystickButton(self.stick, 5)
self.button6 = JoystickButton(self.stick, 6)
self.button7 = JoystickButton(self.stick, 7)
self.button8 = JoystickButton(self.stick, 8)
self.button9 = JoystickButton(self.stick, 9)
self.button10 = JoystickButton(self.stick, 10)
self.button11 = JoystickButton(self.stick, 11)
self.pad1 = JoystickButton(self.gamepad, 1)
self.pad2 = JoystickButton(self.gamepad, 2)
self.pad3 = JoystickButton(self.gamepad, 3)
self.pad4 = JoystickButton(self.gamepad, 4)
self.pad5 = JoystickButton(self.gamepad, 5)
self.pad6 = JoystickButton(self.gamepad, 6)
self.pad7 = JoystickButton(self.gamepad, 7)
self.pad8 = JoystickButton(self.gamepad, 8)
self.pad9 = JoystickButton(self.gamepad, 9)
self.climb2 = ComboButton(self.pad6, self.button6)
self.climb3 = ComboButton(self.pad5, self.button6)
# Test buttons
self.test = ComboButton(self.pad2, self.button6)
# Command hookups
self.trigger.whenPressed(Intake())
self.trigger.whenReleased(NeutralIntake())
# self.button2.whileHeld(Align(3.0))
self.button8.whenPressed(
type(
'', (InstantCommand, ), {
'initialize':
lambda: Command.getRobot().liftElevator.resetEncoders()
}))
self.pad1.whenPressed(SetObjectMode(
IntakeOutput.BallOrHatchMode.HATCH))
self.pad1.whenReleased(SetObjectMode(
IntakeOutput.BallOrHatchMode.BALL))
Command.getRobot().intakeOutput.mode = IntakeOutput.BallOrHatchMode.HATCH if \
self.pad1.get() else IntakeOutput.BallOrHatchMode.HATCH
self.test.whenPressed(TopConditional())
self.pad3.whenPressed(MiddleConditional())
self.pad4.whenPressed(LowConditional())
self.climb2.whenPressed(TwoClimbG())
self.climb3.whenPressed(OneClimbG())
self.pad7.whenPressed(MoveElevator(1))
self.pad8.whenPressed(HighCenter())
@property
def driveStick(self):
return self.stick
@property
def gamePad(self):
return self.gamePad