class Mechanisms(Subsystem):
def __init__(self):
# Verify motor ports when placed on frame
self.intake = WPI_TalonSRX(6)
self.intake_solenoid = DoubleSolenoid(2, 3)
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kOff)
self.gear_shift = DoubleSolenoid(0, 1)
self.gear_shift.set(DoubleSolenoid.Value.kOff)
self.crossbow = WPI_TalonSRX(5)
def set_crossbow(self, speed):
self.crossbow.set(speed)
def get_crossbow(self):
return self.crossbow.get()
def set_intake(self, speed):
self.intake.set(speed)
def pull_intake(self, setting):
self.intake_solenoid.set(setting)
def shift_gears(self, _setting):
self.gear_shift.set(_setting)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
class IntakePneumatics(Subsystem):
"""
The intake pneumatics subsystem is used by the operator to control the intake opening and
closing.
"""
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the solenoids
self.solenoid = DoubleSolenoid(INTAKE_SOLENOID, INTAKE_FORWARD_SOLENOID,
INTAKE_REVERSE_SOLENOID)
# Set the initial state of the solenoid
# self.solenoid.set(DoubleSolenoid.Value.kForward)
self.solenoid.set(DoubleSolenoid.Value.kReverse)
class Mechanisms(Subsystem):
"""
Subsystem with miscellaneous parts of the robot.
Includes many 'getters' and 'setters' for those different parts.
"""
def __init__(self):
# Hardware
self.stopper = DigitalInput(0)
self.crossbow = Spark(0)
self.intake = Spark(1)
self.gear_shift = DoubleSolenoid(0, 1)
self.intake_solenoid = DoubleSolenoid(2, 3)
# Quantities
self.intake_toggle = False
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kOff)
self.gear_shift.set(DoubleSolenoid.Value.kOff)
def set_crossbow(self, speed):
self.crossbow.set(speed)
def get_crossbow(self):
return self.crossbow.get()
def set_intake(self, speed):
self.intake.set(speed)
def pull_intake(self, setting):
self.intake_solenoid.set(setting)
def shift_gears(self, _setting):
self.gear_shift.set(_setting)
def get_stopper(self):
stopperState = self.stopper.get()
if not stopperState:
return True
elif stopperState:
return False
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
class IntakeOutput(Subsystem):
"""
This subsystem controls the Intake or Output of the Panels or Cargo.
"""
def __init__(self):
super().__init__()
self.leftIntakeTalon = WPI_TalonSRX(RobotMap.LEFT_INTAKE_TALON)
self.rightIntakeTalon = WPI_TalonSRX(RobotMap.RIGHT_INTAKE_TALON)
self.intakeVictor = WPI_VictorSPX(RobotMap.END_EFFECTOR_VICTOR)
self.intakeSolenoid = DoubleSolenoid(RobotMap.INTAKE_IN_SOLENOID,
RobotMap.INTAKE_OUT_SOLENOID)
self.placingSolenoid = DoubleSolenoid(RobotMap.PLACE_IN_SOLENOID,
RobotMap.PLACE_OUT_SOLENOID)
self.leftIntakeTalon.configContinuousCurrentLimit(
6, Constants.TIMEOUT_MS)
self.rightIntakeTalon.configContinuousCurrentLimit(
6, Constants.TIMEOUT_MS)
self.setPosition(IntakeOutput.Position.UP)
self.setArmPower(0)
self.mode = None # This cannot be set here because the buttons do not yet exist.
def initDefaultCommand(self):
pass
def setPosition(self, pos):
if pos == IntakeOutput.Position.DOWN:
self.intakeSolenoid.set(DoubleSolenoid.Value.kReverse)
elif pos == IntakeOutput.Position.UP:
self.intakeSolenoid.set(DoubleSolenoid.Value.kForward)
def setArmPower(self, power):
self.leftIntakeTalon.set(-power)
self.rightIntakeTalon.set(power)
def panelPlace(self, putortake):
if putortake == IntakeOutput.PanelPutOrTake.PUT:
self.placingSolenoid.set(DoubleSolenoid.Value.kForward)
elif putortake == IntakeOutput.PanelPutOrTake.TAKE:
self.placingSolenoid.set(DoubleSolenoid.Value.kReverse)
def giveOrTakeCargo(self, giveortake):
if giveortake == IntakeOutput.CargeGiveOrTake.GIVE:
self.intakeVictor.set(ControlMode.PercentOutput, 1)
elif giveortake == IntakeOutput.CargeGiveOrTake.TAKE:
self.intakeVictor.set(ControlMode.PercentOutput, -0.5)
elif giveortake == IntakeOutput.CargeGiveOrTake.NADA:
self.intakeVictor.set(ControlMode.PercentOutput, 0)
def getPistonPos(self):
return self.placingSolenoid.get() == DoubleSolenoid.Value.kForward
def setMode(self, mode):
assert (isinstance(mode, IntakeOutput.BallOrHatchMode))
self.mode = mode
class Position(enum.IntEnum):
DOWN = 0
UP = 1
class PanelPutOrTake(enum.IntEnum):
PUT = 0
TAKE = 1
class CargeGiveOrTake(enum.IntEnum):
NADA = 0
GIVE = 1
TAKE = 2
class BallOrHatchMode(enum.IntEnum):
BALL = 0
HATCH = 1
class IntakeInOutNeutral(enum.IntEnum):
NEUTRAL = 0
IN = 1
OUT = 2