def getJoystick():
"""
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
"""
joystick = Joystick(0)
button5 = JoystickButton(joystick, 5)
button6 = JoystickButton(joystick, 6)
button3 = JoystickButton(joystick, 3)
button4 = JoystickButton(joystick, 4)
button1 = JoystickButton(joystick, 1)
button2 = JoystickButton(joystick, 2)
button7 = JoystickButton(joystick, 7)
button5.whenPressed(BallIntake())
button6.whileHeld(BallOutake())
button3.whileHeld(LiftArm())
button4.whileHeld(LowerArm())
button1.whileHeld(HatchUp())
button2.whileHeld(HatchDown())
button7.whileHeld(FindTarget())
return joystick
def getJoystick():
"""This function creates an instance of the joystick, and assigns commands
to the joystick buttons."""
joystick = Joystick(0)
trigger = JoystickButton(joystick, Joystick.ButtonType.kTrigger)
trigger.whileHeld(GrippersIntake())
thumb = JoystickButton(joystick, Joystick.ButtonType.kTop)
thumb.whileHeld(GrippersExhaust())
POV_up = POVButton(joystick, 0)
POV_down = POVButton(joystick, 180)
POV_up.whileActive(ElevatorUp())
POV_down.whileActive(ElevatorDown())
button_5 = JoystickButton(joystick, 5)
button_5.whenPressed(HandlesState(HandlesOpen(), HandlesClose()))
button_6 = JoystickButton(joystick, 6)
button_6.whenPressed(DrivetrainState(StrengthState(), SpeedState()))
return joystick
def init():
global joystick
joystick = Joystick(0)
crossbow_open = JoystickButton(joystick, 1)
crossbow_open.whenPressed(CrossBow())
def teleopInit(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
buttonA = JoystickButton(self.joystick1, 1)
buttonY = JoystickButton(self.joystick1, 4)
buttonA.whenPressed(self.elevatorIntake)
buttonY.whenPressed(self.elevatorScale)
def getJoystick():
"""
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
"""
joystick = Joystick(0)
trigger = JoystickButton(joystick, Joystick.ButtonType.kTrigger)
trigger.whenPressed(Crash())
return joystick
def init():
'''
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
'''
global joystick
joystick = Joystick(0)
trigger = JoystickButton(joystick, Joystick.ButtonType.kTrigger)
trigger.whenPressed(Crash())
def init():
'''
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
'''
global joystick_player
joystick_player = Joystick(0)
set_joystick()
trigger = JoystickButton(joystick_player, Joystick.ButtonType.kTrigger)
trigger.whenPressed(Record(FollowJoystick()))
class OI(metaclass=singleton.Singleton):
"""Deals with anything controller related,
be it gamepads, joysticks, or steering wheels."""
def __init__(self):
self.driver = joystick.Joystick(Constants.DRIVER_PORT,
Constants.DRIVER_X_MOD,
Constants.DRIVER_Y_MOD,
Constants.DRIVER_Z_MOD)
self.operator = joystick.Joystick(Constants.OPERATOR_PORT,
Constants.OPERATOR_X_MOD,
Constants.OPERATOR_Y_MOD,
Constants.OPERATOR_Z_MOD)
self.hatchbutton = JoystickButton(self.driver, 1)
self.hatchbutton.whenPressed(togglehatchlatch.ToggleHatchLatch())
def teleopInit(self):
'''Called only at the beginning of teleoperated mode'''
#How the buttons for the xbox controller are mapped
self.drivetrain.init_logger()
b = JoystickButton(self.joystick, 1) #A
b2 = JoystickButton(self.joystick, 2) #B
b.whenPressed(self.angle)
b2.cancelWhenPressed(self.angle)
b3 = JoystickButton(self.joystick, 3) #X
b4 = JoystickButton(self.joystick, 4) #Y
b3.whenPressed(self.DriveForward)
b4.cancelWhenPressed(self.DriveForward)
'''
def setup_button_bindings(self):
cmdcfg = configparser.ConfigParser()
cmdcfg.read(self._command_config)
arm_max_position = cmdcfg.getint("ArmCommands", "RAISED_BOUND")
hook_max_position = cmdcfg.getint("HookCommands", "EXTENDED_BOUND")
scoring_right_trigger = JoystickButton(self._controllers[UserController.SCORING], JoystickButtons.RIGHTTRIGGER)
scoring_a_button = JoystickButton(self._controllers[UserController.SCORING], JoystickButtons.A)
scoring_y_button = JoystickButton(self._controllers[UserController.SCORING], JoystickButtons.Y)
scoring_left_trigger = JoystickButton(self._controllers[UserController.SCORING], JoystickButtons.LEFTTRIGGER)
scoring_right_trigger.whenPressed(ShootBall(self.robot))
#scoring_y_button.whenPressed(RaiseArmExtendHook(self.robot, arm_max_position, hook_max_position))
#scoring_a_button.whenPressed(RetractHookToCount(self.robot, 1.0, 0))
scoring_left_trigger.whenPressed(PickUpBall(self.robot, 1.0, "PickUpBall", 5.0))
def init():
"""
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
"""
global leftDriverStick
global rightDriverStick
global resetYawBtn
global autoBtn
global autoTurnBtn
leftDriverStick = T16000M(0)
rightDriverStick = T16000M(1)
resetYawBtn = JoystickButton(rightDriverStick, 2)
resetYawBtn.whenPressed(ResetYawAngle())
autoBtn = JoystickButton(leftDriverStick, 2)
autoBtn.whenPressed(AutoDriveForwardToScale())
def init():
global joystick
joystick = Joystick(robotmap.portsList.stickID)
startFuelOutakeButton = JoystickButton(
joystick, robotmap.buttonsAndAxesList.startFuelOutakeID)
startFuelOutakeButton.whenPressed(
RunFuelOutake(robotmap.speedsList.fuelOutakeSpeed))
stopFuelOutakeButton = JoystickButton(
joystick, robotmap.buttonsAndAxesList.stopFuelOutakeID)
stopFuelOutakeButton.whenPressed(StopFuelOutake())
openGearDoorButton = JoystickButton(joystick,
robotmap.buttonsAndAxesList.openGearID)
openGearDoorButton.whenPressed(OpenGear())
closeGearDoorButton = JoystickButton(
joystick, robotmap.buttonsAndAxesList.closeGearID)
closeGearDoorButton.whenPressed(CloseGear())
def get_joystick():
joystick = Joystick(0)
thumb = JoystickButton(joystick, Joystick.ButtonType.kTop)
thumb.whenPressed(SwitchMode())
trigger = JoystickButton(joystick, Joystick.ButtonType.kTrigger)
trigger.whileHeld(OperateConveyor())
button_11 = JoystickButton(joystick, 11)
button_11.whenPressed(AutoTargetY())
button_12 = JoystickButton(joystick, 12)
button_12.whenPressed(AutoTargetX())
button_9 = JoystickButton(joystick, 9)
button_9.whenPressed(AutoTargetAngle())
return joystick
def init():
global joystick
joystick = Joystick(0)
crossbow = JoystickButton(joystick, 5)
crossbow_in_hold = JoystickButton(joystick, 6)
solenoid_intake = JoystickButton(joystick, 1)
crossbow.whenPressed(Crossbow(0.8, 1))
crossbow_in_hold.whenPressed(Crossbow(-0.6, 1.75))
solenoid_intake.whenPressed(PullIntake())
intake = JoystickButton(joystick, 4)
intake.toggleWhenPressed(Intake())
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))
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)
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
def init():
"""
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
"""
global leftDriverStick
global rightDriverStick
global goGamePad
try:
leftDriverStick = T16000M(0)
except:
print(
'OI: Error - Could not instantiate Left Driver Stick on USB port 0!!!'
)
try:
rightDriverStick = T16000M(1)
except:
print(
'OI: Error - Could not instantiate Right Driver Stick on USB port 0!!!'
)
try:
goGamePad = Joystick(2)
except:
print('OI: Error - Could not instantiate GO GamePad on USB port 2!!!')
# ----------------------------------------------------------
# Driver Controls
# ----------------------------------------------------------
#global resetYawBtn
#resetYawBtn = JoystickButton(rightDriverStick, config.btnResetYawAngleIndex)
#resetYawBtn.whenPressed(NavxResetYawAngle())
#global btnDriveSlow
#btnDriveSlow = JoystickButton(leftDriverStick, config.btnDriveSlow)
global btnEnableLightSensor
btnEnableLightSensor = JoystickButton(leftDriverStick,
config.btnEnableLightSensorIndex)
global btnResetEncoders
btnResetEncoders = JoystickButton(leftDriverStick,
config.btnResetEncodersIndex)
btnResetEncoders.whenPressed(ElevatorResetEncoders())
# ----------------------------------------------------------
# Manipulators (Cargo and Hatch)
# ----------------------------------------------------------
global btnCargoGrabTog
btnCargoGrabTog = JoystickButton(goGamePad, config.btnCargoGrabTogIndex)
btnCargoGrabTog.whenPressed(CargoToggleTrigger())
global btnHatchGrabTog
btnHatchGrabTog = JoystickButton(goGamePad, config.btnHatchGrabTogIndex)
btnHatchGrabTog.whenPressed(HatchToggleTrigger())
global btnExtendSuctionsTog
btnExtendSuctionsTog = JoystickButton(goGamePad,
config.btnExtendSuctionsTogIndex)
btnExtendSuctionsTog.whenPressed(ExtendSuctionsTrigger())
# ----------------------------------------------------------
# Elevator system
# ----------------------------------------------------------
global btnElevatorHatchHeight
btnElevatorHatchHeight = JoystickButton(goGamePad,
config.btnElevatorHatchHeightIndex)
btnElevatorHatchHeight.whenPressed(ElevatorMoveHatchHeight())
global btnElevatorCargoHeight
btnElevatorCargoHeight = JoystickButton(goGamePad,
config.btnElevatorCargoHeightIndex)
btnElevatorCargoHeight.whenPressed(ElevatorCargoHeight())
# ----------------------------------------------------------
# Ramp system
# ----------------------------------------------------------
#global btnRampExtendTog
#btnRampExtendTog = JoystickButton(goGamePad, config.btnRampExtendTogIndex)
#btnRampExtendTog.whenPressed(RampExtend())
#global btnRampRetractTog
#btnRampRetractTog = JoystickButton(goGamePad, config.btnRampRetractTogIndex)
#btnRampRetractTog.whenPressed(RampRetract())
global btnRampTog
btnRampTog = JoystickButton(goGamePad, config.btnRampTogIndex)
btnRampTog.whenPressed(RampToggleTrigger())
# ----------------------------------------------------------
# Lift system
# ----------------------------------------------------------
global btnAutoClimb
btnAutoClimb = JoystickButton(leftDriverStick, config.btnAutoClimbIndex)
btnAutoClimb.whileHeld(AutoClimb())
#global btnTogAllLift
#btnTogAllLift = JoystickButton(rightDriverStick, config.btnTogAllLiftIndex)
#btnTogAllLift.whenPressed(AllLiftTogTrigger())
global btnExtendAll
btnExtendAll = JoystickButton(rightDriverStick, config.btnExtendAllIndex)
btnExtendAll.whenPressed(ExtendAll())
global btnRetractAll
btnRetractAll = JoystickButton(rightDriverStick, config.btnRetractAllIndex)
btnRetractAll.whenPressed(RetractAll())
#global btnTogFrontLift
#btnTogFrontLift = JoystickButton(rightDriverStick, config.btnTogFrontLiftIndex)
#btnTogFrontLift.whenPressed(FrontLiftTogTrigger())
global btnExtendFront
btnExtendFront = JoystickButton(rightDriverStick,
config.btnExtendFrontIndex)
btnExtendFront.whenPressed(ExtendFront())
global btnRetractFront
btnRetractFront = JoystickButton(rightDriverStick,
config.btnRetractFrontIndex)
btnRetractFront.whenPressed(RetractFront())
#global btnTogBackLift
#btnTogBackLift = JoystickButton(rightDriverStick, config.btnTogBackLiftIndex)
#btnTogBackLift.whenPressed(BackLiftTogTrigger())
global btnExtendBack
btnExtendBack = JoystickButton(rightDriverStick, config.btnExtendBackIndex)
btnExtendBack.whenPressed(ExtendBack())
global btnRetractBack
btnRetractBack = JoystickButton(rightDriverStick,
config.btnRetractBackIndex)
btnRetractBack.whenPressed(RetractBack())
def init():
global joystick
joystick = Joystick(0)
# JoystickButton(joystick, buttons.L_BUMPER).whenPressed(GearShift(Gearing.LOW))
# JoystickButton(joystick, buttons.R_BUMPER).whenPressed(GearShift(Gearing.HIGH))
# #JoystickButton(joystick, buttons.SQUARE).whenPressed(Grabber(True))
# JoystickButton(joystick, buttons.X).whenPressed(Grabber("toggle"))
gearup = JoystickButton(joystick, 1)
geardown = JoystickButton(joystick, 2)
grabberclose = JoystickButton(joystick, 4)
grabberopen = JoystickButton(joystick, 3)
shooterdown = JoystickButton(joystick, 5)
shooterup = JoystickButton(joystick, 6)
homebutton = JoystickButton(joystick, 13)
# 5 and 6 are L and R bumpers
gearup.whenPressed(GearShift(Gearing.HIGH))
geardown.whenPressed(GearShift(Gearing.LOW))
grabberclose.whenPressed(Grabber(True))
grabberopen.whenPressed(Grabber(False))
shooterup.whenPressed(Shooter(True))
shooterdown.whenPressed(Shooter(False))
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")
def init():
"""
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
"""
global leftDriverStick
global rightDriverStick
global goGamePad
try:
leftDriverStick = T16000M(0)
except:
print('OI: Error - Could not instantiate Left Driver Stick on USB port 0!!!')
try:
rightDriverStick = T16000M(1)
except:
print('OI: Error - Could not instantiate Right Driver Stick on USB port 0!!!')
try:
goGamePad = Joystick(2)
except:
print('OI: Error - Could not instantiate Right Driver Stick on USB port 2!!!')
# ----------------------------------------------------------
# Driver Controls
# ----------------------------------------------------------
#global resetYawBtn
#resetYawBtn = JoystickButton(rightDriverStick, config.btnResetYawAngleIndex)
#resetYawBtn.whenPressed(NavxResetYawAngle())
global btnDriveSlow
btnDriveSlow = JoystickButton(leftDriverStick, config.btnDriveSlow)
global btnEnableLightSensor
btnEnableLightSensor = JoystickButton(leftDriverStick, config.btnEnableLightSensorIndex)
global btnExtendAll
btnExtendAll = JoystickButton(rightDriverStick, config.btnExtendAllIndex)
btnExtendAll.whenPressed(ExtendAll())
global btnRetract
btnRetract = JoystickButton(rightDriverStick, config.btnRetractAllIndex)
btnRetract.whenPressed(RetractAll())
global btnExtendFront
btnExtendFront = JoystickButton(rightDriverStick, config.btnExtendFrontIndex)
btnExtendFront.whenPressed(ExtendFront())
global btnExtendBack
btnExtendBack = JoystickButton(rightDriverStick, config.btnExtendBackIndex)
btnExtendBack.whenPressed(ExtendBack())
global btnRetractFront
btnRetractFront = JoystickButton(rightDriverStick, config.btnRetractFrontIndex)
btnRetractFront.whenPressed(RetractFront())
global btnCargoGrabTog
btnCargoGrabTog = JoystickButton(goGamePad, config.btnHatchGrabTogIndex)
btnCargoGrabTog.whenPressed(ExtendBack())
"""
global btnResetEncoders
btnResetEncoders = JoystickButton(leftDriverStick, config.btnResetEncodersIndex)
btnResetEncoders.whenPressed(TankDriveResetEncoders())
"""
"""
global axisElevator
axisElevator = JoystickAxis(goGamePad, config.axisElevatorIndex)
axisElevator. #??? idk how to configure joystick axis
"""
"""
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