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