class ToggleIntakeCommand(Command):
def __init__(self, intake: Intake):
super().__init__()
self.last_arm_state = intake.get_arm_state()
self.new_arm_state = ArmState.DOWN
self.requires(intake)
self.intake = intake
self.arm_timer = Timer()
self.arm_timer_latch = False
def initialize(self):
self.last_arm_state = self.intake.get_arm_state()
self.new_arm_state = ArmState.DOWN if self.last_arm_state == ArmState.UP else ArmState.UP
if self.last_arm_state != ArmState.UP:
self.arm_timer.start()
self.arm_timer.reset()
self.arm_timer_latch = True
def execute(self):
intake = self.intake
intake.set_arm_state(self.new_arm_state)
self.last_arm_state = self.intake.get_arm_state()
if self.arm_timer.get() > 0.5:
self.arm_timer_latch = False
self.arm_timer.stop()
if self.arm_timer_latch:
intake.run_intake(0.1)
else:
intake.run_intake(0)
def isFinished(self):
return not self.arm_timer_latch
def end(self):
self.arm_timer.reset()
self.arm_timer.stop()
self.arm_timer_latch = False
self.intake.run_intake(0)
class MoveIntakeCommand(Command):
def __init__(self, intake: Intake, new_state: ArmState):
super().__init__("MoveIntakeCommand")
self.intake = intake
self.new_state = new_state
self.old_state = None
self.timer = Timer()
self.requires(intake)
def initialize(self):
self.old_state = self.intake.get_arm_state()
self.intake.set_arm_state(self.new_state)
self.timer.reset()
self.timer.start()
def isFinished(self):
return self.new_state == self.old_state or \
(self.new_state == ArmState.DOWN and self.timer.get() > 0.2) or \
(self.new_state == ArmState.UP and self.timer.get() > 0.5)
def end(self):
self.timer.stop()
class Pitchfork(TimedRobot):
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.startSpotChooser = SendableChooser()
self.startSpotChooser.addObject("Start Left", 'Left')
self.startSpotChooser.addObject("Start Right", 'Right')
self.startSpotChooser.addDefault("Start Middle", 'Middle')
self.smartDashboard.putData("Starting Spot", self.startSpotChooser)
self.scaleDisableChooser = SendableChooser()
self.scaleDisableChooser.addObject("Enable Scale", 'Scale')
self.scaleDisableChooser.addDefault("Disable Scale", 'No Scale')
self.smartDashboard.putData("Scale Enable", self.scaleDisableChooser)
# Build up the autonomous dictionary. Fist key is the starting position. The second key is the switch. The third key is the scale.
self.chooserOptions = {"Left": {"R": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
"L": {"R": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
},
},
"Middle": {"R": {"R": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
},
},
"Right": {"R": {"R": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
"L": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
},
}
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
#===========================================================================================
# if LOGGER_LEVEL == logging.INFO:
# self.smartDashboard.putNumber("rVelocity",
# self.driveTrain.getRightVelocity())
# self.smartDashboard.putNumber("lVelocity",
# self.driveTrain.getLeftVelocity())
# self.smartDashboard.putNumber("rVoltage",
# self.driveTrain.getRightVoltage())
# self.smartDashboard.putNumber("lVoltage",
# self.driveTrain.getLeftVoltage())
# self.smartDashboard.putNumber("TimeStamp",
# self.timer.get())
#===========================================================================================
def disabledInit(self):
"""
Initialization code for disabled mode should go here. This method will be called each
time the robot enters disabled mode.
"""
self.timer.stop()
self.timer.reset()
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here. This method will be called every 20ms.
"""
if self.timer.running:
self.timer.stop()
self.timer.reset()
def robotPeriodic(self):
pass
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
self.startingPosition = self.startSpotChooser.getSelected()
self.scaleDisable = self.scaleDisableChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
logger.info("Game Data: %s" % (self.gameData))
logger.info("Starting Position %s" % (self.startingPosition))
logger.info("Scale Enable %s" % (self.scaleDisable))
self.autonCommand = self.chooserOptions[self.startingPosition][self.gameData[0]][self.gameData[1]][self.scaleDisable]['command'](self)
self.autonCommand.start()
def autonomousPeriodic(self):
"""
Periodic code for autonomous mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
def teleopInit(self):
"""
Initialization code for teleop mode should go here. This method will be called each time
the robot enters teleop mode.
"""
if not self.timer.running:
self.timer.start()
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
class Intake(Component):
def __init__(self):
super().__init__()
self._l_motor = Talon(hardware.intake_left)
self._r_motor = Talon(hardware.intake_right)
self._intake_piston = Solenoid(hardware.intake_solenoid)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
self._left_intake_amp = CircularBuffer(25)
self._right_intake_amp = CircularBuffer(25)
self._pulse_timer = Timer()
def update(self):
self._l_motor.set(self._left_pwm)
self._r_motor.set(-self._right_pwm)
self._intake_piston.set(not self._open)
self._left_intake_amp.append(hardware.left_intake_current())
self._right_intake_amp.append(hardware.right_intake_current())
def intake_tote(self):
if hardware.game_piece_in_intake(): # anti-bounce & slowdown
power = .8
else:
power = .3 if not hardware.back_photosensor.get() else 1
self.set(power)
if self.intake_jammed() and not self._pulse_timer.running:
self._pulse_timer.start()
if self._pulse_timer.running:
self.set(-self._pulse_timer.get() / 2)
if self._pulse_timer.get() > .05:
self._pulse_timer.stop()
self._pulse_timer.reset()
def intake_bin(self):
power = .3 if not hardware.back_photosensor.get() else .65
self.set(power, power)
def pause(self):
self.set(0)
def open(self):
self._open = True
def close(self):
self._open = False
def stop(self):
self._l_motor.set(0)
self._r_motor.set(0)
def set(self, l, r=None):
""" Spins the intakes at a given power. Pass either a power or left and right power. """
self._left_pwm = l
if r is None:
self._right_pwm = l
else:
self._right_pwm = r
def intake_jammed(self):
if not hardware.back_photosensor.get():
return False
return self._left_intake_amp.average > AMP_THRESHOLD or self._right_intake_amp.average > AMP_THRESHOLD
def update_nt(self):
log.info("left current: %s" % self._left_intake_amp.average)
log.info("right current: %s" % self._right_intake_amp.average)
class Pitchfork(TimedRobot):
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.scoringElementChooser = SendableChooser()
self.scoreScale = ScoreScale()
self.scoreSwitch = ScoreSwitch()
self.scoringElementChooser.addObject("Score Scale", self.scoreScale)
self.scoringElementChooser.addObject("Score Switch", self.scoreSwitch)
self.scoringElementChooser.addDefault("Score Scale", self.scoreScale)
self.smartDashboard.putData("Score Field Element",
self.scoringElementChooser)
self.crossFieldChooser = SendableChooser()
self.crossFieldEnable = CrossFieldEnable()
self.crossFieldDisable = CrossFieldDisable()
self.crossFieldChooser.addObject("Cross Field Enable",
self.crossFieldEnable)
self.crossFieldChooser.addObject("Cross Field Disable",
self.crossFieldDisable)
self.crossFieldChooser.addDefault("Cross Field Disable",
self.crossFieldDisable)
self.smartDashboard.putData("Cross Field Enable",
self.crossFieldChooser)
self.positionChooser = SendableChooser()
self.startingLeft = StartingLeft()
self.startingRight = StartingRight()
self.startingMiddle = StartingMiddle()
self.positionChooser.addObject("Start Left", self.startingLeft)
self.positionChooser.addObject("Start Right", self.startingRight)
self.positionChooser.addObject("Start Middle", self.startingMiddle)
self.positionChooser.addDefault("Start Middle", self.startingMiddle)
self.smartDashboard.putData("Starting Position", self.positionChooser)
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
self.autonForward = AutonForward(self)
self.autonMiddleStartLeftSwitch = AutonMiddleStartLeftSwitch(self)
self.autonLeftStartLeftScale = AutonLeftStartLeftScale(self)
# Output debug data to the smartdashboard
if LOGGER_LEVEL == logging.DEBUG:
#=======================================================================================
# self.smartDashboard.putNumber("RightEncPos", 0.0)
# self.smartDashboard.putNumber("RightActPos", 0.0)
# self.smartDashboard.putNumber("RightEncVel", 0.0)
# self.smartDashboard.putNumber("RightActVel", 0.0)
# self.smartDashboard.putNumber("RightPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("RightPrimaryError", 0.0)
# self.smartDashboard.putNumber("TimeStamp", 0.0)
# self.smartDashboard.putNumber("LeftEncPos", 0.0)
# self.smartDashboard.putNumber("LeftActPos", 0.0)
# self.smartDashboard.putNumber("LeftEncVel", 0.0)
# self.smartDashboard.putNumber("LeftActVel", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryError", 0.0)
# self.smartDashboard.putNumber("RightTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftBottomBufferCount", 0.0)
# self.smartDashboard.putNumber("RightBottomBufferCount", 0.0)
#=======================================================================================
self.smartDashboard.putNumber("EncPos", 0.0)
self.smartDashboard.putNumber("ActPos", 0.0)
self.smartDashboard.putNumber("EncVel", 0.0)
self.smartDashboard.putNumber("ActVel", 0.0)
self.smartDashboard.putNumber("PrimaryTarget", 0.0)
self.smartDashboard.putNumber("PrimaryError", 0.0)
self.smartDashboard.putNumber("TimeStamp", 0.0)
def disabledInit(self):
"""
Initialization code for disabled mode should go here. This method will be called each
time the robot enters disabled mode.
"""
self.timer.stop()
self.timer.reset()
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here. This method will be called every 20ms.
"""
if self.timer.running:
self.timer.stop()
self.timer.reset()
def robotPeriodic(self):
pass
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# Get the prioritized scoring element, robot starting posion, and the alliance
# scale/switch data.
self.selectedCrossFieldEnable = self.crossFieldChooser.getSelected()
self.selectedScoringElement = self.scoringElementChooser.getSelected()
self.selectedStartingPosition = self.positionChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
self.crossFieldEnable = self.selectedCrossFieldEnable.getCrossFieldEnable(
)
self.scoringElement = self.selectedScoringElement.getScoringElement()
self.startingPosition = self.selectedStartingPosition.getStartingPosition(
)
def autonomousPeriodic(self):
"""
Periodic code for autonomous mode should go here. This method will be called every 20ms.
"""
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
if self.scheduleAutonomous:
self.scheduleAutonomous = False
logger.info("Game Data: %s" % (self.gameData))
logger.info("Cross Field Enable: %s" % (self.crossFieldEnable))
logger.info("Scoring Element %s" % (self.scoringElement))
logger.info("Starting Position %s" % (self.startingPosition))
self.autonMiddleStartLeftSwitch.start()
#self.autonLeftStartLeftScale.start()
#self.autonForward.start()
Scheduler.getInstance().run()
def teleopInit(self):
"""
Initialization code for teleop mode should go here. This method will be called each time
the robot enters teleop mode.
"""
if not self.timer.running:
self.timer.start()
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
class OpIntakeCommand(Command):
def __init__ (self, intake: Intake):
super().__init__ ()
self.requires(intake)
self.intake = intake
self.last_arm_state = ArmState.UP
self.arm_timer = Timer()
self.arm_timer_latch = False
def initialize(self):
self.last_arm_state = self.intake.get_arm_state()
self.arm_timer.reset()
self.arm_timer.stop()
self.arm_timer_latch = False
def execute(self):
oi = OI.get()
intake = self.intake
if oi.arm_is_down():
intake.set_arm_state(ArmState.DOWN)
else:
intake.set_arm_state(ArmState.UP)
if self.last_arm_state != ArmState.UP:
self.arm_timer.start()
self.arm_timer.reset()
self.arm_timer_latch = True
self.last_arm_state = self.intake.get_arm_state()
if self.arm_timer.get() > 0.5:
self.arm_timer_latch = False
self.arm_timer.stop()
if self.arm_timer_latch:
intake.run_intake(0.1)
else:
self.arm_timer.stop()
if oi.intake_is_active():
# intake.set_grab_state(GrabState.OUT)
pwr = oi.get_outtake_command()
if abs(pwr) < 0.05:
pwr = 0
max_intake_power = 0.5
if pwr > max_intake_power:
pwr = max_intake_power
intake.run_intake(pwr)
if intake.has_acquired_cube() and pwr > 0:
oi.rumble_op()
else:
oi.unrumble_op()
else:
intake.run_intake(0)
oi.unrumble_op()
if oi.arm_is_open():
intake.set_grab_state(GrabState.OUT)
else:
intake.set_grab_state(GrabState.IN)
def end(self):
OI.get().unrumble_op()
def isFinished(self):
return False
