def __init__(self, robot):
'''The Constructor - assign Xbox controller buttons to specific Commands.
'''
print("In OI:__init__")
robot.xbox0 = wpilib.XboxController(0)
robot.xbox1 = wpilib.XboxController(1)
"""liftleft = JoystickButton(robot.xbox1, XboxController.Button.kA)
lowerleft = JoystickButton(robot.xbox1, XboxController.Button.kB)
liftright = JoystickButton(robot.xbox1, XboxController.Button.kX)
lowerright = JoystickButton(robot.xbox1, XboxController.Button.kY)
liftfront = JoystickButton(robot.xbox1, XboxController.Button.kStickLeft)
lowerfront = JoystickButton(robot.xbox1, XboxController.Button.kStickRight)
liftrear = JoystickButton(robot.xbox1, XboxController.Button.kBumperRight)
lowerrear = JoystickButton(robot.xbox1, XboxController.Button.kBumperLeft)
lift = JoystickButton(robot.xbox1, XboxController.Button.kStart)
lower = JoystickButton(robot.xbox1, XboxController.Button.kBack)"""
#claw = JoystickButton(robot.xbox1, XboxController.Button.kY)
intake = JoystickButton(robot.xbox1, XboxController.Button.kA)
liftwinch = JoystickButton(robot.xbox1,
XboxController.Button.kBumperRight)
lowerwinch = JoystickButton(robot.xbox1,
XboxController.Button.kBumperLeft)
ejectcargo = JoystickButton(robot.xbox1, XboxController.Button.kX)
triggerbutton = TriggerButton(robot.xbox0, .1)
punch = JoystickButton(robot.xbox0, XboxController.Button.kY)
punchrear = JoystickButton(robot.xbox1, XboxController.Button.kY)
hatch = JoystickButton(robot.xbox0, XboxController.Button.kX)
#park = JoystickButton(robot.xbox0, XboxController.Button.kA)
"""liftleft.whileHeld(LiftLeft(robot))
lowerleft.whileHeld(LowerLeft(robot))
liftright.whileHeld(LiftRight(robot))
lowerright.whileHeld(LowerRight(robot))
liftfront.whileHeld(LiftFront(robot))#
lowerfront.whileHeld(LowerFront(robot#
liftrear.whileHeld(LiftRear(robot))
lowerrear.whileHeld(LowerRear(robot))#
lift.whileHeld(Lift(robot))
lower.whileHeld(Lower(robot))"""
triggerbutton.whenPressed(MoveArmWithTriggers(robot))
intake.toggleWhenPressed(IntakeCargo(robot))
ejectcargo.toggleWhenPressed(EjectCargo(robot))
#claw.toggleWhenPressed(OpenClaw(robot))
punch.whenPressed(Punch(robot))
punch.whenReleased(Pull(robot))
hatch.toggleWhenPressed(CoverHatch(robot))
liftwinch.whileHeld(LiftWinch(robot))
lowerwinch.whileHeld(LowerWinch(robot))
punchrear.whenPressed(PunchRear(robot))
punchrear.whenReleased(PullRear(robot))
def __init__(self, robot):
'''The Constructor - assign Xbox controller buttons to specific Commands.
'''
print("In OI:__init__")
robot.xbox0 = wpilib.XboxController(0)
robot.xbox1 = wpilib.XboxController(1)
hatch = JoystickButton(robot.xbox0, XboxController.Button.kX)
park = JoystickButton(robot.xbox0, XboxController.Button.kBack)
togglecamera = JoystickButton(robot.xbox0,
XboxController.Button.kStart)
triggerbutton = TriggerButton(robot.xbox0, .1)
stickbutton = StickButton(robot.xbox0, .1)
ejectcargo = JoystickButton(robot.xbox1, XboxController.Button.kX)
intakecargo = JoystickButton(robot.xbox1, XboxController.Button.kA)
frontlift = JoystickButton(robot.xbox1, XboxController.Button.kStart)
rearlift = JoystickButton(robot.xbox1, XboxController.Button.kBack)
invertfront = JoystickButton(robot.xbox1,
XboxController.Button.kStickRight)
liftwinch = JoystickButton(robot.xbox1,
XboxController.Button.kBumperRight)
lowerwinch = JoystickButton(robot.xbox1,
XboxController.Button.kBumperLeft)
punchrear = JoystickButton(robot.xbox1, XboxController.Button.kY)
hatch.toggleWhenPressed(CoverHatch(robot))
park.whileHeld(Park(robot))
togglecamera.whenPressed(ToggleCamera(robot))
triggerbutton.whenPressed(MoveArmWithTriggers(robot))
stickbutton.whenPressed(DifferentialDriveWithXbox(robot))
ejectcargo.toggleWhenPressed(EjectCargo(robot))
intakecargo.toggleWhenPressed(IntakeCargo(robot))
frontlift.toggleWhenPressed(LiftFront(robot))
rearlift.toggleWhenPressed(LiftRear(robot))
invertfront.toggleWhenPressed(InvertFront(robot))
liftwinch.whileHeld(LiftWinch(robot))
lowerwinch.whileHeld(LowerWinch(robot))
punchrear.whenReleased(PullRear(robot))
punchrear.whenPressed(PunchRear(robot))
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
Command.getRobot = lambda x=0: self
self.driveSubsystem = DriveSubsystem()
self.controller = Joystick(0)
self.forward = JoystickButton(self.controller, PS4Button["TRIANGLE"])
self.backward = JoystickButton(self.controller, PS4Button["CROSS"])
self.right = JoystickButton(self.controller, PS4Button["CIRCLE"])
self.left = JoystickButton(self.controller, PS4Button["SQUARE"])
self.forward.whenPressed(MoveForward())
self.forward.whenReleased(Stop())
self.backward.whenPressed(MoveBackward())
self.backward.whenReleased(Stop())
self.right.whenPressed(TurnRight())
self.right.whenReleased(Stop())
self.left.whenPressed(TurnLeft())
self.left.whenReleased(Stop())
def autonomousInit(self):
'''Called only at the beginning of autonomous mode'''
pass
def autonomousPeriodic(self):
'''Called every 20ms in autonomous mode'''
pass
def disabledInit(self):
'''Called only at the beginning of disabled mode'''
pass
def disabledPeriodic(self):
'''Called every 20ms in disabled mode'''
pass
def teleopInit(self):
'''Called only at the beginning of teleoperated mode'''
pass
def teleopPeriodic(self):
'''Called every 20ms in teleoperated mode'''
Scheduler.getInstance().run()
def init():
global driverOne
global driverTwo
driverOne = XboxController(robotmap.USB_PORT_DRIVER_ONE)
driverTwo = XboxController(robotmap.USB_PORT_DRIVER_TWO)
toggleLauncher = JoystickButton(driverTwo, robotmap.BUTTON_A)
toggleLauncher.whenPressed(LaunchHatchCommand())
toggleLauncher.whenReleased(RetractCommand())
toggleFront = JoystickButton(driverTwo, robotmap.BUTTON_RIGHT_BUMPER)
toggleFront.whenReleased(ToggleFrontCommand())
toggleRear = JoystickButton(driverTwo, robotmap.BUTTON_LEFT_BUMPER)
toggleRear.whenReleased(ToggleRearCommand())
def __init__ (self, robot): robot.joystick = wpilib.Joystick(0) robot.joystick1 = wpilib.Joystick(1) L3_button = JoystickButton(robot.joystick1, 9) LB_button = JoystickButton(robot.joystick1, 5) A_button = JoystickButton(robot.joystick1, 1) B_button = JoystickButton(robot.joystick1, 2) X_button = JoystickButton(robot.joystick1, 3) Y_button = JoystickButton(robot.joystick1, 4) L3_button.whenPressed(TurboDrive()) L3_button.whenReleased(FollowJoystick()) LB_button.whenPressed(Align()) LB_button.whenReleased(FollowJoystick()) X_button.whenPressed(MoveClawTo(power=-.5)) X_button.whenReleased(MoveClawTo(power=0)) B_button.whenPressed(MoveClawTo(power=.5)) B_button.whenReleased(MoveClawTo(power=0)) Y_button.whenPressed(MoveArmTo(power=-1)) Y_button.whenReleased(MoveArmTo(power=0)) A_button.whenPressed(MoveArmTo(power=1)) A_button.whenReleased(MoveArmTo(power=0))
class OI:
"""
OI defines the operator interface for Ares.
- two joysticks with many associated buttons
- smartdashboard widgets
"""
# Joysticks
k_driveStickPort = 0
k_launcherStickPort = 1
# DriveStick Commands - controls for driver:
# move, turn, throttle, intake, drive, intake ball
k_intakeBallButton = 3
k_aimIntakeButton = 4
k_driveStopIntakeWheelsButton = 5
k_aimNeutralButton = 6
k_turnThrottleButton = 8
k_driveStrightButton = 11 # experimental
# LauncherStick Commands - controls for mechanism operator:
# aim, shoot, portcullis
k_lightSwitchButton = 2
k_kickBallButton = 3
k_spinIntakeWheelsOutButton = 4
k_stopIntakeWheelsButton = 5
k_pcUpButton = 6 # Pc is portcullis
k_pcDownButton = 7
k_pcBarStopButton = 9
k_pcBarInButton = 10
k_pcBarOutButton = 11
# Auto positions are numbered (1-5), position 1 is special
k_LowBarPosition = 1
# Auto barrier
k_LowBarBarrier = 0
k_MoatBarrier = 1
k_RoughTerrainBarrier = 2
k_RockWallBarrier = 3
k_PortcullisBarrier = 4
k_RampartsBarrier = 5
# Auto strategy
k_NoMoveStrategy = 0
k_CrossStrategy = 1
k_CrossBlindShotStrategy = 2
k_CrossVisionShotStrategy = 3
def __init__(self, robot):
self.robot = robot
self.driveStick = wpilib.Joystick(OI.k_driveStickPort)
self.aimStick = wpilib.Joystick(OI.k_launcherStickPort)
self.initDriveTrain()
self.initLauncher()
self.initPortcullis()
self.initAutonomous()
self.initVision()
self.initSmartDashboard()
def initDriveTrain(self):
ds = self.driveStick
self.dtTurnThrottleButton = JoystickButton(ds, OI.k_turnThrottleButton)
# TODO: convert whileHeld/whenRealsed pair to a single command
self.dtTurnThrottleButton.whileHeld(DTCmds.TurnSpeedSlow(self.robot))
self.dtTurnThrottleButton.whenReleased(DTCmds.TurnSpeedFast(self.robot))
def initLauncher(self):
aS = self.aimStick
dS = self.driveStick
self.kickBallButton = JoystickButton(aS, OI.k_kickBallButton)
self.kickBallButton.whenPressed(ILCmds.LaunchBallCommandGroup(self.robot))
# TODO: Java code had two buttons for stop wheels
self.mechStopWheelsButton = JoystickButton(aS, OI.k_stopIntakeWheelsButton)
self.mechStopWheelsButton.whenPressed(ILCmds.StopWheelsCommand(self.robot))
self.grabBallButton = JoystickButton(dS, OI.k_intakeBallButton)
self.grabBallButton.whenPressed(ILCmds.IntakeBallCommandGroup(self.robot))
self.spinIntakeWheelsOutButton = JoystickButton(aS, OI.k_spinIntakeWheelsOutButton)
self.spinIntakeWheelsOutButton.whenPressed(ILCmds.SpinIntakeWheelsCommand(self.robot, "out"))
self.driveLauncherJumpToIntakeButton = JoystickButton(dS, OI.k_aimIntakeButton)
self.driveLauncherJumpToIntakeButton.whenPressed(ILCmds.LauncherGoToIntakeCommand(self.robot))
# TODO: Java code had two buttons for neutral
self.driveLauncherJumpToNeutralButton = JoystickButton(aS, OI.k_aimNeutralButton)
self.driveLauncherJumpToNeutralButton.whenPressed(ILCmds.LauncherGoToNeutralCommand(self.robot))
def initVision(self):
s = self.aimStick
self.lightSwitchButton = JoystickButton(s, OI.k_lightSwitchButton)
self.lightSwitchButton.whenPressed(VCmds.LightSwitchCmd(self.robot))
def initPortcullis(self):
s = self.aimStick
self.pcUpButton = JoystickButton(s, OI.k_pcUpButton)
self.pcUpButton.whenPressed(PCmds.MoveUp(self.robot))
self.pcDownButton = JoystickButton(s, OI.k_pcDownButton)
self.pcDownButton.whenPressed(PCmds.MoveDown(self.robot))
self.pcBarInButton = JoystickButton(s, OI.k_pcBarInButton)
self.pcBarInButton.whenPressed(PCmds.BarIn(self.robot))
self.pcBarOutButton = JoystickButton(s, OI.k_pcBarOutButton)
self.pcBarOutButton.whenPressed(PCmds.BarOut(self.robot))
self.pcBarStopButton = JoystickButton(s, OI.k_pcBarStopButton)
self.pcBarStopButton.whenPressed(PCmds.BarStop(self.robot))
def initAutonomous(self):
ch = SendableChooser()
ch.addDefault("1: Low Bar", self.k_LowBarPosition)
ch.addObject("2", 2)
ch.addObject("3", 3)
ch.addObject("4", 4)
ch.addObject("5", 5)
self.startingFieldPosition = ch
ch = SendableChooser()
ch.addDefault("Low Bar", self.k_LowBarBarrier)
ch.addObject("Moat", self.k_MoatBarrier)
ch.addObject("Rough Terrain", self.k_RoughTerrainBarrier)
ch.addObject("Rock Wall", self.k_RockWallBarrier)
ch.addObject("Portcullis", self.k_PortcullisBarrier)
ch.addObject("Ramparts", self.k_RampartsBarrier)
self.barrierType = ch
ch = SendableChooser()
ch.addDefault("None", self.k_NoMoveStrategy)
ch.addObject("Cross Only", self.k_CrossStrategy)
ch.addObject("Cross, Blind Shot", self.k_CrossBlindShotStrategy)
ch.addObject("Cross, Vision Shot",self.k_CrossVisionShotStrategy)
self.strategy = ch
def initSmartDashboard(self):
SmartDashboard.putData("AutoFieldPosition", self.startingFieldPosition)
SmartDashboard.putData("AutoBarrierType", self.barrierType)
SmartDashboard.putData("AutoStrategy", self.strategy)
