class Intake: def __init__(self, Robot): self._lift = Robot.lift self._ramp = 0.0 self._rampSpeed = 6.0 self._intakeMotorLeft = WPI_VictorSPX(*INTAKE_MOTOR_LEFT_IDS) self._intakeMotorRight = WPI_VictorSPX(*INTAKE_MOTOR_RIGHT_IDS) self._intakeMotorRight.setInverted(True) self._intakePistons = Solenoid(INTAKE_PISTONS) self._photosensor = LimelightPhotosensor(Robot.limelight, 1) def rampSpeed(self, speed): # Ramp to prevent brown outs self._ramp += (speed - self._ramp) / self._rampSpeed self._intakeMotorLeft.set(self._ramp) self._intakeMotorRight.set(self._ramp) def setSpeed(self, speed): self._intakeMotorLeft.set(speed) self._intakeMotorRight.set(speed) def getSpeed(self): return self._intakeMotorLeft.get() def pistonToggle(self): self._intakePistons.set(not self._intakePistons.get()) def hasCube(self): return self._photosensor.isTriggered() def periodic(self): #if lift.isBottom(): #photosensor.update() pass
class MyRobot(wpilib.TimedRobot): """ Small program to verify Neo motor and encoder are working. """ def robotInit(self): self.solenoid = Solenoid(0, 1) def teleopInit(self): self.solenoid.set(True)
class Popper(Subsystem): def __init__(self, can_id=0, channel=0): super().__init__() self.solenoid = Solenoid(can_id, channel) def set(self, state): self.solenoid.set(state) def extend(self): self.set(True) def retract(self): self.set(False)
class lift(Component): def __init__(self, robot): super().__init__() self.robot = robot self.cylinder1Up = Solenoid(6, 4) self.cylinder1Down = Solenoid(6, 5) self.cylinder2Up = Solenoid(6, 6) self.cylinder2Down = Solenoid(6, 7) self.ARM_STATUS = False self.BOTH_HELD = False def climbUpDown(self, leftButtonPressed, rightButtonPressed): if(rightButtonPressed == True and leftButtonPressed == True and self.BOTH_HELD == False): self.BOTH_HELD = True # Toggle arm status if(self.ARM_STATUS == False): self.ARM_STATUS = True elif(self.ARM_STATUS == True): self.ARM_STATUS = False # Set cylinder to up or down... Don't break plz!!! self.cylinder1Up.set(self.ARM_STATUS) self.cylinder1Down.set(not self.ARM_STATUS) self.cylinder2Up.set(self.ARM_STATUS) self.cylinder2Down.set(not self.ARM_STATUS) # Reset the button held flag elif(rightButtonPressed == False and leftButtonPressed == False): self.BOTH_HELD = False
class Intake(Component): def __init__(self): super().__init__() self._l_motor = Talon(constants.motor_intake_l) self._r_motor = Talon(constants.motor_intake_r) self._intake_piston = Solenoid(constants.solenoid_intake) self._photosensor = DigitalInput(constants.far_photosensor) self._left_pwm = 0 self._right_pwm = 0 self._open = False def update(self): self._l_motor.set(self._left_pwm) self._r_motor.set(self._right_pwm) self._intake_piston.set(not self._open) def spin(self, power, same_direction=False): self._left_pwm = power self._right_pwm = power * (1 if same_direction else -1) def intake_tote(self): power = .3 if not self._photosensor.get() else .75 self.spin(power) def intake_bin(self): power = .3 if not self._photosensor.get() else .65 self.spin(power) def open(self): self._open = True def close(self): self._open = False def stop(self): """Disables EVERYTHING. Only use in case of critical failure""" self._l_motor.set(0) self._r_motor.set(0)
class PneumaticsSubsystem(Subsystem): def __init__(self): self.mainCompressor = Compressor(PCMID) self.intakeSolenoid = Solenoid(PCMID, intakeSolenoidChannel) self.isExtended = False self.mainCompressor.start() def initDefaultCommand(self): pass def flipSolenoid(self): if self.isExtended == False: self.isExtended = True else: self.isExtended == False self.intakeSolenoid.set(isExtended) def getIsExtended(self): return self.isExtended
class ClawSubsystem(Subsystem): def __init__(self, robot): super().__init__("Claw") self.robot = robot self.leftMotor = Spark(robotmap.CLAWLEFT) self.rightMotor = Spark(robotmap.CLAWRIGHT) self.CLAW_OPEN = Solenoid(robotmap.PCM1_CANID, robotmap.CLAW_OPEN_SOLENOID) self.CLAW_CLOSE = Solenoid(robotmap.PCM1_CANID, robotmap.CLAW_CLOSE_SOLENOID) self.sensor = InfraredSensor(robotmap.INFRARED_SENSOR_CHANNEL) self.close() def initDefaultCommand(self): self.setDefaultCommand(ObtainBoxContinuous(self.robot)) def open(self): self.CLAW_OPEN.set(True) self.CLAW_CLOSE.set(False) def close(self): self.CLAW_OPEN.set(False) self.CLAW_CLOSE.set(True) def setSpeed(self, pullSpeed): #positive left speed is pull self.leftMotor.set(pullSpeed) self.rightMotor.set(-pullSpeed) def boxIsClose(self): return self.sensor.GetMedianVoltage() > 0.6 def boxIsReallyClose(self): return self.sensor.GetMedianVoltage() > 2.6
class ColorWheel(): def __init__(self): self.colorWheelExtend = Solenoid(robotmap.PCM_ID, robotmap.COLOR_WHEEL_EXTEND_SOLENOID) self.colorWheelRetract = Solenoid( robotmap.PCM_ID, robotmap.COLOR_WHEEL_RETRACT_SOLENOID) self.colorWheelMotor = Spark(robotmap.COLOR_WHEEL_ID) self.RGBSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard) self.colorWheelExtend.set(False) self.colorWheelRetract.set(True) def DeployWheelArm(self): self.colorWheelExtend.set(True) self.colorWheelRetract.set(False) def StowWheelArm(self): self.colorWheelExtend.set(False) self.colorWheelRetract.set(True) def Iterate(self, copilot: xboxcontroller.XBox): self.color = self.RGBSensor.getColor() self.ir = self.RGBSensor.getIR() self.proxy = self.RGBSensor.getProximity( ) #proxy's still alive Jazz band if self.color.red > 0.7 and self.color.red <= 1: self.red = True elif self.color.green > 0.7 and self.color.green <= 1: self.green = True elif self.color.blue > 0.7 and self.color.blue <= 1: self.blue = True else: self.red = False self.green = False self.blue = False #Boolean wpilib.SmartDashboard.putBoolean("Red", self.red) wpilib.SmartDashboard.putBoolean("Green", self.green) wpilib.SmartDashboard.putBoolean("Blue", self.blue) #Value wpilib.SmartDashboard.putNumber("RedN", self.color.red) wpilib.SmartDashboard.putNumber("BlueN", self.color.blue) wpilib.SmartDashboard.putNumber("GreenN", self.color.green) wpilib.SmartDashboard.putNumber("IR", self.ir) wpilib.SmartDashboard.putNumber("Proxy", self.proxy) #print(self.color) if copilot.X(): self.colorWheelMotor.set(0.5) elif copilot.B(): self.colorWheelMotor.set(-0.5) else: self.colorWheelMotor.stopMotor() if copilot.getDPadRight(): self.DeployWheelArm() elif copilot.getDPadLeft(): self.StowWheelArm()
class HatchMech(Subsystem): def __init__(self, Robot): """ Create all physical parts used by subsystem. """ super().__init__('Hatch') self.debug = True self.robot = Robot def initialize(self): self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) self.joystick1 = oi.getJoystick(1) self.kicker = Solenoid(map.hatchKick) self.slider = Solenoid(map.hatchSlide) self.kick("in") self.slide("in") self.lastKick = False self.lastSlide = False def periodic(self): #if self.xbox.getRawButton(map.kickHatch) == True: self.kick("out") #elif self.xbox.getRawButton(map.toggleHatch) == True: self.kick("in") #if self.xbox.getRawButton(map.extendHatch) == True: self.slide("out") #elif self.xbox.getRawButton(map.retractHatch) == True: self.slide("in") currKick = self.xbox.getRawButton(map.kickHatch) currSlide = self.xbox.getRawButton(map.toggleHatch) if currKick and currKick != self.lastKick: self.kick("toggle") if currSlide and currSlide != self.lastSlide: self.slide("toggle") self.lastKick = currKick self.lastSlide = currSlide if self.joystick0.getRawButton( map.drivehatch) or self.joystick1.getRawButton(map.drivehatch): self.kick("out") def kick(self, mode): if mode == "out": self.kicker.set(True) elif mode == "in": self.kicker.set(False) else: self.kicker.set(not self.kicker.get()) def slide(self, mode): if mode == "out": self.slider.set(True) elif mode == "in": self.slider.set(False) else: self.slider.set(not self.slider.get()) def isEjectorOut(self): return self.kicker.get() def toggle(self): if self.kicker.get(): self.kick("out") else: self.kick("in") def disable(self): self.kick("in") def dashboardInit(self): pass def dashboardPeriodic(self): pass
class HatchMech(Subsystem): def __init__(self, Robot): #Create all physical parts used by subsystem. super().__init__('Hatch') #self.debug = True self.robot = Robot def initialize(self): #makes control objects self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) self.joystick1 = oi.getJoystick(1) #makes solenoid objects to be used in kick and slide functions self.kicker = Solenoid(map.hatchKick) self.slider = Solenoid(map.hatchSlide) self.maxVolts = 10 timeout = 0 self.wheels = Talon(map.hatchWheels) self.wheels.setNeutralMode(2) self.wheels.configVoltageCompSaturation(self.maxVolts) self.wheels.configContinuousCurrentLimit(20,timeout) #20 Amps per motor self.wheels.configPeakCurrentLimit(30,timeout) #30 Amps during Peak Duration self.wheels.configPeakCurrentDuration(100,timeout) #Peak Current for max 100 ms self.wheels.enableCurrentLimit(True) self.wheels.setInverted(True) self.powerIn = 0.9 self.powerOut = -0.9 #sets kicker and slide solenoids to in self.kick("in") self.slide("in") #starts lastkick/slide booleans self.lastKick = False self.lastSlide = False self.hasHatch = False self.outPower = 0 def periodic(self): self.updateHatch() self.currKick = self.xbox.getRawButton(map.kickHatch) self.currSlide = self.xbox.getRawButton(map.toggleHatch) #if the variable is true and it does not equal the lastkick/slide boolean, sets it to the opposite of what it currently is if self.currKick and (self.currKick != self.lastKick): self.kick("toggle") if self.currSlide and (self.currSlide != self.lastSlide): self.slide("toggle") #after the if statement, sets the lastkick/slide to the currkick/slide self.lastKick = self.currKick self.lastSlide = self.currSlide self.setWheels() # kick function to activate kicker solenoid def kick(self, mode): #out mode sets kicker solenoid to true if mode == "out": self.setKick(True) #in mode sets kicker solenoid to false elif mode == "in": self.setKick(False) #if neither of them, makes the kicker solenoid to the opposite of what it is else: self.setKick(not self.kicker.get()) # slide function to activate slide solenoid def slide(self, mode): # out mode sets slider solenoid to true if mode == "out": self.slider.set(True) # in mode sets slider solenoid to false elif mode == "in": self.slider.set(False) #if neither of them, makes the slider solenoid to the opposite of what it is else: self.slider.set(not self.slider.get()) def setKick(self, state): self.kicker.set(state) if state and self.slider.get() and self.hasHatch: self.hasHatch = False def setWheels(self): if self.kicker.get() and self.slider.get() and self.hasHatch: self.wheels.set(self.powerOut) self.outPower = self.powerOut elif not self.kicker.get() and self.slider.get() and not self.hasHatch: #should be "and not self.hasHatch" self.wheels.set(self.powerIn) self.outPower = self.powerIn #elif self.joystick1.getRawButton(1): # self.wheels.set(self.powerIn) # self.outPower = self.powerIn else: self.wheels.set(0) self.outPower = 0 def updateHatch(self): #only checks current to possibly set false to true for hasHatch threshold = 10 #10 amp current separates freely spinning and stalled if self.slider.get() and self.wheels.getOutputCurrent()>threshold: self.hasHatch = True if self.joystick0.getRawButton(3) or self.joystick0.getRawButton(4) or self.joystick1.getRawButton(3) or self.joystick1.getRawButton(4): self.hasHatch = True #disable function runs kick function on in def disable(self): self.kick("in") def dashboardInit(self): pass def dashboardPeriodic(self): #commented out some values. DON'T DELETE THESE VALUES #SmartDashboard.putNumber("Hatch Current", self.wheels.getOutputCurrent()) #SmartDashboard.putNumber("Power", self.outPower) SmartDashboard.putBoolean("Has Hatch", self.hasHatch) SmartDashboard.putBoolean("Slider Out", self.slider.get()) SmartDashboard.putBoolean("Kicker Out", self.kicker.get())
class Climb(): def __init__(self): self.climbExtend = Solenoid(robotmap.PCM_ID, robotmap.CLIMB_EXTEND_SOLENOID) self.climbRetract = Solenoid(robotmap.PCM_ID, robotmap.CLIMB_RETRACT_SOLENOID) self.hookExtend = Solenoid(robotmap.PCM_ID, robotmap.HOOK_EXTEND_SOLENOID) self.hookRetract = Solenoid(robotmap.PCM_ID, robotmap.HOOK_RETRACT_SOLENOID) self.hookReleaseExtend = Solenoid(robotmap.PCM2_ID, robotmap.HOOK_RELEASE_EXTEND_SOLENOID) self.hookReleaseRetract = Solenoid(robotmap.PCM2_ID, robotmap.HOOK_RELEASE_RETRACT_SOLENOID) self.climbMotor = ctre.WPI_VictorSPX(robotmap.LIFT_WINCH_ID) self.climbExtend.set(False) self.climbRetract.set(True) self.hookExtend.set(False) self.hookRetract.set(True) self.hookReleaseExtend.set(True) self.hookReleaseRetract.set(False) def DeployWinch(self): self.climbExtend.set(True) self.climbRetract.set(False) def StowWinch(self): self.climbExtend.set(False) self.climbRetract.set(True) def DeployHook(self): self.hookExtend.set(True) self.hookRetract.set(False) def StowHook(self): self.hookExtend.set(False) self.hookRetract.set(True) def ReleaseHook(self): self.hookReleaseExtend.set(False) self.hookReleaseRetract.set(True) def HoldHook(self): self.hookReleaseExtend.set(True) self.hookReleaseRetract.set(False) def config(self): pass def Iterate(self, pilot: XBox): if pilot.getDPadUp(): self.DeployWinch() elif pilot.getDPadDown(): self.StowWinch() if pilot.RightBumper(): self.DeployHook() elif pilot.LeftBumper(): self.StowHook() if pilot.Start(): self.HoldHook() elif pilot.Back(): self.ReleaseHook() if pilot.Y(): self.climbMotor.set(1) elif pilot.A(): self.climbMotor.set(-1) else: self.climbMotor.stopMotor()
class Pneumatics(Subsystem): def __init__(self): super().__init__("Pneumatics") self.solenoid_R = Solenoid(robotmap.solenoid_R) self.solenoid_L = Solenoid(robotmap.solenoid_L) self.is_active = False def extend(self): self.solenoid_L.set(False) self.solenoid_R.set(True) def retract(self): self.solenoid_R.set(False) self.solenoid_L.set(True) def halt(self): self.solenoid_R.set(False) self.solenoid_L.set(False) def get_active(self): return self.is_active def alternate(self): pass
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 DeepSpaceLift(): min_lift_position = ntproperty("/LiftSettings/MinLiftPosition", 75, persistent=True) max_lift_position = ntproperty("/LiftSettings/MaxLiftPosition", 225, persistent=True) max_lift_adjust_rate = ntproperty("/LiftSettings/MaxLiftAdjustRate", 10, persistent=True) max_lift_adjust_value = ntproperty("/LiftSettings/MaxLiftAdjustValue", 15, persistent=True) lift_stow_position = ntproperty("/LiftSettings/LiftStowPosition", 90, persistent=True) lift_front_port_low = ntproperty("/LiftSettings/LiftFrontPortLow", 150, persistent=True) lift_front_port_middle = ntproperty("/LiftSettings/LiftFrontPortMiddle", 175, persistent=True) lift_front_port_high = ntproperty("/LiftSettings/LiftFrontPortHigh", 200, persistent=True) lift_side_hatch_low = ntproperty("/LiftSettings/LiftSideHatchLow", 135, persistent=True) lift_side_hatch_middle = ntproperty("/LiftSettings/LiftSideHatchMiddle", 160, persistent=True) lift_side_hatch_high = ntproperty("/LiftSettings/LiftSideHatchHigh", 185, persistent=True) on_target = False def __init__(self, logger): self.logger = logger def init(self): self.logger.info("DeepSpaceLift::init()") self.timer = wpilib.Timer() self.timer.start() self.robot_mode = RobotMode.TEST self.last_lift_adjust_time = 0 self.lift_adjust_timer = wpilib.Timer() self.lift_adjust_timer.start() self.state_timer = wpilib.Timer() self.current_state = LiftState.LIFT_START_CONFIGURATION self.current_lift_preset = LiftPreset.LIFT_PRESET_STOW self.current_lift_preset_val = self.lift_stow_position self.lift_setpoint = self.min_lift_position self.lift_adjust_val = 0 self.lift_talon = TalonSRX(robotmap.LIFT_CAN_ID) '''Select and zero sensor in init() function. That way the zero position doesn't get reset every time we enable/disable robot''' self.lift_talon.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0, robotmap.CAN_TIMEOUT_MS) self.lift_talon.setSelectedSensorPosition(0, 0, robotmap.CAN_TIMEOUT_MS) self.lift_pneumatic_extend = Solenoid(robotmap.PCM1_CANID, robotmap.LIFT_RAISE_SOLENOID) self.lift_pneumatic_retract = Solenoid(robotmap.PCM1_CANID, robotmap.LIFT_LOWER_SOLENOID) self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) self.test_lift_pneumatic = sendablechooser.SendableChooser() self.test_lift_pneumatic.setDefaultOption("Retract", 1) self.test_lift_pneumatic.addOption("Extend", 2) SmartDashboard.putData("TestLiftPneumatic", self.test_lift_pneumatic) def config(self, simulation): self.logger.info("DeepSpaceLift::config()") '''Generic config''' self.lift_talon.configNominalOutputForward(0.0, robotmap.CAN_TIMEOUT_MS) self.lift_talon.configNominalOutputReverse(0.0, robotmap.CAN_TIMEOUT_MS) self.lift_talon.configPeakOutputForward(1.0, robotmap.CAN_TIMEOUT_MS) self.lift_talon.configPeakOutputReverse(-1.0, robotmap.CAN_TIMEOUT_MS) self.lift_talon.enableVoltageCompensation(True) self.lift_talon.configVoltageCompSaturation(11.5, robotmap.CAN_TIMEOUT_MS) self.lift_talon.configOpenLoopRamp(0.125, robotmap.CAN_TIMEOUT_MS) self.lift_talon.setInverted(True) '''sensor config''' self.lift_talon.configSelectedFeedbackCoefficient( 1.0, 0, robotmap.CAN_TIMEOUT_MS) self.lift_talon.setSensorPhase(False) self.lift_talon.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 10, robotmap.CAN_TIMEOUT_MS) def deploy_lift(self): self.current_state = LiftState.LIFT_DEPLOY_BEGIN def go_to_preset(self, preset): self.lift_adjust_val = 0 if preset == LiftPreset.LIFT_PRESET_PORT_LOW: self.current_lift_preset = LiftPreset.LIFT_PRESET_PORT_LOW self.current_lift_preset_val = self.lift_front_port_low self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) self.state_timer.stop() self.state_timer.reset() elif preset == LiftPreset.LIFT_PRESET_PORT_MIDDLE: self.current_lift_preset = LiftPreset.LIFT_PRESET_PORT_MIDDLE self.current_lift_preset_val = self.lift_front_port_middle self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) self.state_timer.stop() self.state_timer.reset() elif preset == LiftPreset.LIFT_PRESET_PORT_HIGH: self.current_lift_preset = LiftPreset.LIFT_PRESET_PORT_HIGH self.current_lift_preset_val = self.lift_front_port_high self.lift_pneumatic_extend.set(True) self.lift_pneumatic_retract.set(False) self.state_timer.stop() self.state_timer.reset() elif preset == LiftPreset.LIFT_PRESET_HATCH_LOW: self.current_lift_preset = LiftPreset.LIFT_PRESET_HATCH_LOW self.current_lift_preset_val = self.lift_side_hatch_low self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) self.state_timer.stop() self.state_timer.reset() elif preset == LiftPreset.LIFT_PRESET_HATCH_MIDDLE: self.current_lift_preset = LiftPreset.LIFT_PRESET_HATCH_MIDDLE self.current_lift_preset_val = self.lift_side_hatch_middle self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) self.state_timer.stop() self.state_timer.reset() elif preset == LiftPreset.LIFT_PRESET_HATCH_HIGH: self.current_lift_preset = LiftPreset.LIFT_PRESET_HATCH_HIGH self.current_lift_preset_val = self.lift_side_hatch_high self.lift_pneumatic_extend.set(True) self.lift_pneumatic_retract.set(False) self.state_timer.stop() self.state_timer.reset() elif preset == LiftPreset.LIFT_PRESET_STOW: self.current_lift_preset = LiftPreset.LIFT_PRESET_STOW self.current_lift_preset_val = self.lift_stow_position self.state_timer.reset() self.state_timer.start() def iterate(self, robot_mode, simulation, pilot_stick, copilot_stick): self.robot_mode = robot_mode lift_position = self.lift_talon.getAnalogInRaw() if lift_position > self.lift_stow_position + 5: SmartDashboard.putBoolean("Creep", True) else: SmartDashboard.putBoolean("Creep", False) if robot_mode == RobotMode.TEST: if self.test_lift_pneumatic.getSelected() == 1: self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) else: self.lift_pneumatic_extend.set(True) self.lift_pneumatic_retract.set(False) #need to check these separately so we don't disable the mechanism completely if we end up one tick outside our allowable range if lift_position > self.min_lift_position or lift_position < self.max_lift_position: self.lift_talon.set( ControlMode.PercentOutput, -1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS)) elif lift_position < self.min_lift_position: #allow upward motion self.lift_talon.set( ControlMode.PercentOutput, max( -1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0)) elif lift_position > self.max_lift_position: #allow downward motion self.lift_talon.set( ControlMode.PercentOutput, min( -1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0)) else: self.lift_talon.set(ControlMode.PercentOutput, 0.0) else: self.iterate_state_machine(pilot_stick, copilot_stick) SmartDashboard.putString("Lift State", self.current_state.name) SmartDashboard.putString("Lift Preset", self.current_lift_preset.name) SmartDashboard.putNumber("Lift Setpoint", self.lift_setpoint) SmartDashboard.putNumber("Lift Position", lift_position) def iterate_state_machine(self, pilot_stick, copilot_stick): #****************DEPLOY SEQUENCE************************** if self.current_state == LiftState.LIFT_DEPLOY_EXTEND_PNEUMATIC: self.lift_pneumatic_extend.set(True) self.lift_pneumatic_retract.set(False) self.state_timer.reset() self.state_timer.start() self.current_state = LiftState.LIFT_DEPLOY_WAIT1 elif self.current_state == LiftState.LIFT_DEPLOY_WAIT1: if self.state_timer.hasPeriodPassed(0.5): self.current_state = LiftState.LIFT_DEPLOY_MOVE_TO_STOW elif self.current_state == LiftState.LIFT_DEPLOY_MOVE_TO_STOW: self.go_to_preset(LiftPreset.LIFT_PRESET_STOW) if self.bang_bang_to_setpoint(): self.current_state = LiftState.LIFT_DEPLOY_RETRACT_PNEUMATIC if self.current_state == LiftState.LIFT_DEPLOY_RETRACT_PNEUMATIC: self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) self.state_timer.reset() self.state_timer.start() self.current_state = LiftState.LIFT_DEPLOY_WAIT2 elif self.current_state == LiftState.LIFT_DEPLOY_WAIT2: if self.state_timer.hasPeriodPassed(0.25): self.current_state = LiftState.LIFT_DEPLOYED #****************DEPLOY SEQUENCE************************** #****************DEPLOYED********************************* elif self.current_state == LiftState.LIFT_DEPLOYED: if self.robot_mode == RobotMode.TELE: if copilot_stick.LeftBumper().get() and copilot_stick.X().get( ): if not self.current_lift_preset == LiftPreset.LIFT_PRESET_PORT_LOW: self.go_to_preset(LiftPreset.LIFT_PRESET_PORT_LOW) elif copilot_stick.LeftBumper().get() and copilot_stick.Y( ).get(): if not self.current_lift_preset == LiftPreset.LIFT_PRESET_PORT_MIDDLE: self.go_to_preset(LiftPreset.LIFT_PRESET_PORT_MIDDLE) elif copilot_stick.LeftBumper().get() and copilot_stick.B( ).get(): if not self.current_lift_preset == LiftPreset.LIFT_PRESET_PORT_HIGH: self.go_to_preset(LiftPreset.LIFT_PRESET_PORT_HIGH) elif copilot_stick.RightBumper().get() and copilot_stick.X( ).get(): if not self.current_lift_preset == LiftPreset.LIFT_PRESET_HATCH_LOW: self.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_LOW) elif copilot_stick.RightBumper().get() and copilot_stick.Y( ).get(): if not self.current_lift_preset == LiftPreset.LIFT_PRESET_HATCH_MIDDLE: self.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_MIDDLE) elif copilot_stick.RightBumper().get() and copilot_stick.B( ).get(): if not self.current_lift_preset == LiftPreset.LIFT_PRESET_HATCH_HIGH: self.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_HIGH) else: if not self.current_lift_preset == LiftPreset.LIFT_PRESET_STOW: self.go_to_preset(LiftPreset.LIFT_PRESET_STOW) current_lift_adjust_time = self.lift_adjust_timer.get() dt = current_lift_adjust_time - self.last_lift_adjust_time self.last_lift_adjust_time = current_lift_adjust_time if self.bang_bang_to_setpoint(): max_down_adjust = float( abs(self.current_lift_preset_val - self.lift_stow_position)) max_up_adjust = float( abs(self.current_lift_preset_val - self.max_lift_position)) self.lift_adjust_val += dt * self.max_lift_adjust_rate * pilot_stick.RightStickY( ) if self.lift_adjust_val > 0: self.lift_adjust_val = min(self.lift_adjust_val, self.max_lift_adjust_value) else: self.lift_adjust_val = max(self.lift_adjust_val, -self.max_lift_adjust_value) self.lift_adjust_val = max( min(self.lift_adjust_val, max_up_adjust), -max_down_adjust) #****************DEPLOYED********************************* def bang_bang_to_setpoint(self): if self.current_lift_preset == LiftPreset.LIFT_PRESET_STOW and ( self.state_timer.get() > 1.0 or self.robot_mode == RobotMode.AUTO): self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) self.set_lift_setpoint(self.current_lift_preset_val + int(self.lift_adjust_val)) if not self.current_lift_preset == LiftPreset.LIFT_PRESET_STOW: self.set_lift_setpoint(self.current_lift_preset_val + int(self.lift_adjust_val)) lift_position = self.lift_talon.getSelectedSensorPosition(0) err = abs(lift_position - self.lift_setpoint) if err > 1: self.on_target = False if lift_position < self.lift_setpoint: self.lift_talon.set(ControlMode.PercentOutput, 1.0) elif lift_position > self.lift_setpoint: self.lift_talon.set(ControlMode.PercentOutput, -1.0) else: self.lift_talon.set(ControlMode.PercentOutput, 0.0) self.on_target = True return self.on_target def disable(self): self.logger.info("DeepSpaceLift::disable()") self.lift_talon.set(ControlMode.PercentOutput, 0) def set_lift_setpoint(self, ticks): self.lift_setpoint = max(min(ticks, self.max_lift_position), self.min_lift_position)
class Drive: def __init__(self, Robot): #self.navx = Robot.navx self.speedLimit = 0.999 self._leftRamp = 0.0 self._rightRamp = 0.0 self._rampSpeed = 6.0 self._kOonBalanceAngleThresholdDegrees = 5.0 self._autoBalance = True self._quickstop_accumulator = 0.0 self._old_wheel = 0.0 self._driveReversed = True self._drivePool = DataPool("Drive") # setup drive train motors self.rightDrive = SyncGroup(RIGHT_DRIVE_MOTOR_IDS, WPI_VictorSPX) self.leftDrive = SyncGroup(LEFT_DRIVE_MOTOR_IDS, WPI_VictorSPX) self.rightDrive.setInverted(True) # setup drive train gear shifter self.shifter = Solenoid(DRIVE_SHIFTER_PORT) self.shifter.set(False) # setup drive train encoders self.leftEncoder = Encoder(LEFT_DRIVE_ENCODER_A, LEFT_DRIVE_ENCODER_B, False, Encoder.EncodingType.k4X) self.rightEncoder = Encoder(RIGHT_DRIVE_ENCODER_A, RIGHT_DRIVE_ENCODER_B, False, Encoder.EncodingType.k4X) self.leftEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK) self.leftEncoder.setReverseDirection(True) self.rightEncoder.setReverseDirection(False) self.rightEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK) def cheesyDrive(self, wheel, throttle, quickturn, altQuickturn, shift): throttle = Util.deadband(throttle) wheel = Util.deadband(wheel) if self._driveReversed: wheel *= -1 neg_inertia = wheel - self._old_wheel self._old_wheel = wheel wheel = Util.sinScale(wheel, 0.9, 1, 0.9) if wheel * neg_inertia > 0: neg_inertia_scalar = 2.5 else: if abs(wheel) > .65: neg_inertia_scalar = 6 else: neg_inertia_scalar = 4 neg_inertia_accumulator = neg_inertia * neg_inertia_scalar wheel += neg_inertia_accumulator if altQuickturn: if abs(throttle) < 0.2: alpha = .1 self._quickstop_accumulator = ( 1 - alpha) * self._quickstop_accumulator + alpha * self.limit( wheel, 1.0) * 5 over_power = -wheel * .75 angular_power = -wheel * 1 elif quickturn: if abs(throttle) < 0.2: alpha = .1 self._quickstop_accumulator = ( 1 - alpha) * self._quickstop_accumulator + alpha * self.limit( wheel, 1.0) * 5 over_power = 1 angular_power = -wheel * 1 else: over_power = 0 sensitivity = .9 angular_power = throttle * wheel * sensitivity - self._quickstop_accumulator self._quickstop_accumulator = Util.wrap_accumulator( self._quickstop_accumulator) if shift: if not self.shifter.get(): self.shifter.set(True) else: if self.shifter.get(): self.shifter.set(False) right_pwm = left_pwm = throttle left_pwm += angular_power right_pwm -= angular_power if left_pwm > 1: right_pwm -= over_power * (left_pwm - 1) left_pwm = 1 elif right_pwm > 1: left_pwm -= over_power * (right_pwm - 1) right_pwm = 1 elif left_pwm < -1: right_pwm += over_power * (-1 - left_pwm) left_pwm = -1 elif right_pwm < -1: left_pwm += over_power * (-1 - right_pwm) right_pwm = -1 if self._driveReversed: left_pwm *= -1 right_pwm *= -1 if self.shifter.get(): # if low gear #leftDrive.set(left_pwm) #rightDrive.set(right_pwm) self.moveRamped(left_pwm, right_pwm) else: self.moveRamped(left_pwm, right_pwm) def setGear(self, gear): if self.shifter.get() != gear: self.shifter.set(gear) def tankDrive(self, left, right): scaledBalance = self.autoBalance() left = self.limit(left + scaledBalance, self.speedLimit) right = self.limit(right + scaledBalance, self.speedLimit) self.leftDrive.set(left * LEFT_DRIFT_OFFSET) self.rightDrive.set(right * RIGHT_DRIFT_OFFSET) def limit(self, wheel, d): return Util.limit(wheel, d) def moveRamped(self, desiredLeft, desiredRight): self._leftRamp += (desiredLeft - self._leftRamp) / self._rampSpeed self._rightRamp += (desiredRight - self._rightRamp) / self._rampSpeed self.tankDrive(self._leftRamp, self._rightRamp) def autoShift(self, b): if self.shifter.get() != b: self.shifter.set(b) def periodic(self): self._drivePool.logDouble("gyro_angle", self.getRotation()) self._drivePool.logDouble("left_enc", self.rightEncoder.getDistance()) self._drivePool.logDouble("right_enc", self.leftEncoder.getDistance()) def setDrivetrainReversed(self, rev): self.driveReversed = rev def driveReversed(self): return self.driveReversed def getRotation(self): return self.navx.getAngle() def getLeftDistance(self): return self.leftEncoder.getDistance() * 2.54 * ROBOT_INVERTED def getRightDistance(self): return self.rightEncoder.getDistance() * 2.54 * ROBOT_INVERTED def resetDistance(self): self.leftEncoder.reset() self.rightEncoder.reset() def autoBalance(self): '''if self._autoBalance: pitchAngleDegrees = self.navx.getPitch() scaledPower = 1 + (0 - pitchAngleDegrees - self._kOonBalanceAngleThresholdDegrees) / self._kOonBalanceAngleThresholdDegrees if scaledPower > 2: scaledPower = 2 #return scaledPower;''' return 0 def setSpeedLimit(self, speedLimit): self.speedLimit = self.limit(speedLimit, DRIVE_SPEED_LIMIT)
class DriveSubsystem(Subsystem): def __init__(self, robot): super().__init__("Drive") self.robot = robot self.left_master = WPI_TalonSRX(robotmap.DRIVELEFTMASTER) self.right_master = WPI_TalonSRX(robotmap.DRIVERIGHTMASTER) self.left_slave = WPI_TalonSRX(robotmap.DRIVELEFTSLAVE) self.right_slave = WPI_TalonSRX(robotmap.DRIVERIGHTSLAVE) self.shifter_high = Solenoid(robotmap.PCM1_CANID, robotmap.SHIFTER_HIGH) self.shifter_low = Solenoid(robotmap.PCM1_CANID, robotmap.SHIFTER_LOW) self.differential_drive = DifferentialDrive(self.left_master, self.right_master) self.TalonConfig() self.shiftLow() def teleDrive(self, xSpeed, zRotation): if self.robot.oi.getController1().B().get(): scale = SmartDashboard.getNumber("creep_mult", 0.3) xSpeed = xSpeed * scale zRotation = zRotation * scale self.differential_drive.arcadeDrive(xSpeed, zRotation, False) def getLeftPosition(self): return self.left_master.getSelectedSensorPosition(0) def getRightPosition(self): return self.right_master.getSelectedSensorPosition(0) def getRightSpeed(self): return self.right_master.getSelectedSensorVelocity(0) def getLeftSpeed(self): return self.unitsToInches( self.left_master.getSelectedSensorVelocity(0)) def getErrorLeft(self): return self.left_master.getClosedLoopError(0) def getErrorRight(self): return self.right_master.getClosedLoopError(0) def isLeftSensorConnected(self): return self.left_master.getSensorCollection( ).getPulseWidthRiseToRiseUs() != 0 def isRightSensorConnected(self): return self.right_master.getSensorCollection( ).getPulseWidthRiseToRiseUs() != 0 def resetEncoders(self): self.left_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS) self.right_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS) self.left_master.getSensorCollection().setQuadraturePosition( 0, robotmap.TIMEOUT_MS) self.right_master.getSensorCollection().setQuadraturePosition( 0, robotmap.TIMEOUT_MS) def setMotionMagicLeft(self, setpoint): SmartDashboard.putNumber("setpoint_left_units", self.inchesToUnits(setpoint)) self.left_master.set(ControlMode.MotionMagic, self.inchesToUnits(setpoint)) def setMotionMagicRight(self, setpoint): self.right_master.set(ControlMode.MotionMagic, self.inchesToUnits(-setpoint)) def isMotionMagicNearTarget(self): retval = False if self.left_master.getActiveTrajectoryPosition( ) == self.left_master.getClosedLoopTarget(0): if self.right_master.getActiveTrajectoryPosition( ) == self.right_master.getClosedLoopTarget(0): if abs(self.left_master.getClosedLoopError(0)) < 300: if abs(self.right_master.getClosedLoopError(0)) < 300: retval = True return retval def shiftHigh(self): self.shifter_high.set(True) self.shifter_low.set(False) def shiftLow(self): self.shifter_high.set(False) self.shifter_low.set(True) def stop(self): self.left_master.set(ControlMode.PercentOutput, 0.0) self.right_master.set(ControlMode.PercentOutput, 0.0) def unitsToInches(self, units): return units * robotmap.DRIVE_WHEEL_CIRCUMFERENCE / robotmap.DRIVE_ENCODER_PPR def inchesToUnits(self, inches): return inches * robotmap.DRIVE_ENCODER_PPR / robotmap.DRIVE_WHEEL_CIRCUMFERENCE def autoInit(self): self.resetEncoders() self.left_master.setNeutralMode(NeutralMode.Brake) self.left_slave.setNeutralMode(NeutralMode.Brake) self.right_master.setNeutralMode(NeutralMode.Brake) self.right_slave.setNeutralMode(NeutralMode.Brake) self.differential_drive.setSafetyEnabled(False) self.shiftLow() def teleInit(self): self.left_master.setNeutralMode(NeutralMode.Coast) self.left_slave.setNeutralMode(NeutralMode.Coast) self.right_master.setNeutralMode(NeutralMode.Coast) self.right_slave.setNeutralMode(NeutralMode.Coast) self.differential_drive.setSafetyEnabled(True) self.shiftLow() def TalonConfig(self): self.left_master.configSelectedFeedbackSensor( FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotmap.TIMEOUT_MS) self.right_master.configSelectedFeedbackSensor( FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotmap.TIMEOUT_MS) self.left_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS) self.right_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS) self.left_master.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, robotmap.TIMEOUT_MS) self.left_master.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, robotmap.TIMEOUT_MS) self.right_master.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, robotmap.TIMEOUT_MS) self.right_master.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, robotmap.TIMEOUT_MS) self.left_master.setSensorPhase(False) self.right_master.setSensorPhase(False) self.left_master.setInverted(True) self.left_slave.setInverted(True) self.right_master.setInverted(True) self.right_slave.setInverted(True) #Makes talons force zero voltage across when zero output to resist motion self.left_master.setNeutralMode(NeutralMode.Brake) self.left_slave.setNeutralMode(NeutralMode.Brake) self.right_master.setNeutralMode(NeutralMode.Brake) self.right_slave.setNeutralMode(NeutralMode.Brake) self.left_slave.set(ControlMode.Follower, robotmap.DRIVELEFTMASTER) self.right_slave.set(ControlMode.Follower, robotmap.DRIVERIGHTMASTER) #Closed Loop Voltage Limits self.left_master.configNominalOutputForward(0.0, robotmap.TIMEOUT_MS) self.right_master.configNominalOutputForward(0.0, robotmap.TIMEOUT_MS) self.left_master.configNominalOutputReverse(-0.0, robotmap.TIMEOUT_MS) self.right_master.configNominalOutputReverse(-0.0, robotmap.TIMEOUT_MS) self.left_master.configPeakOutputForward(1.0, robotmap.TIMEOUT_MS) self.right_master.configPeakOutputForward(1.0, robotmap.TIMEOUT_MS) self.left_master.configPeakOutputReverse(-1.0, robotmap.TIMEOUT_MS) self.right_master.configPeakOutputReverse(-1.0, robotmap.TIMEOUT_MS) def configGains(self, f, p, i, d, izone, cruise, accel): self.left_master.selectProfileSlot(0, 0) self.left_master.config_kF(0, f, robotmap.TIMEOUT_MS) self.left_master.config_kP(0, p, robotmap.TIMEOUT_MS) self.left_master.config_kI(0, i, robotmap.TIMEOUT_MS) self.left_master.config_kD(0, d, robotmap.TIMEOUT_MS) self.left_master.config_IntegralZone(0, izone, robotmap.TIMEOUT_MS) self.right_master.selectProfileSlot(0, 0) self.right_master.config_kF(0, f, robotmap.TIMEOUT_MS) self.right_master.config_kP(0, p, robotmap.TIMEOUT_MS) self.right_master.config_kI(0, i, robotmap.TIMEOUT_MS) self.right_master.config_kD(0, d, robotmap.TIMEOUT_MS) self.right_master.config_IntegralZone(0, izone, robotmap.TIMEOUT_MS) self.left_master.configMotionCruiseVelocity(cruise, robotmap.TIMEOUT_MS) self.left_master.configMotionAcceleration(accel, robotmap.TIMEOUT_MS) self.right_master.configMotionCruiseVelocity(cruise, robotmap.TIMEOUT_MS) self.right_master.configMotionAcceleration(accel, robotmap.TIMEOUT_MS)
class Elevator(Component): ON_TARGET_DELTA = 1 / 4 def __init__(self): super().__init__() self._motor = SyncGroup(Talon, hardware.elevator) self._stabilizer_piston = Solenoid(hardware.stabilizer_solenoid) # Motion Planning! self._follower = TrajectoryFollower() self._calibrated = False self.tote_count = 0 self.has_bin = False # Do we have a bin? self._reset = True # starting a new stack? self.tote_first = False # We're stacking totes without a bin. self._should_drop = False # Are we currently trying to get a bin ? self._manual_stack = False self._cap = False self._follower.set_goal(Setpoints.BIN) # Base state self._follower_thread = Thread(target=self.update_follower) self._follower_thread.start() def stop(self): self._motor.set(0) def update(self): goal = self._follower.get_goal() if self.at_goal(): self.do_stack_logic(goal) self._motor.set(self._follower.output) self._stabilizer_piston.set(not self._should_drop) self.tote_first = False self._manual_stack = False self._cap = False def do_stack_logic(self, goal): if self._should_drop: # Dropping should override everything else self.reset_stack() if not hardware.game_piece_in_intake(): self._follower._max_acc = 100 # Put things down gently if there's space before the bottom tote else: self._follower._max_acc = 100000000000 self._follower.set_goal(Setpoints.BOTTOM) self._should_drop = False return self._follower._max_acc = 200 # Normal speed # TODO configurable if goal == Setpoints.BOTTOM: # If we've just gone down to grab something if self.tote_count == 0 and not self.has_bin and not self.tote_first: self.has_bin = True # We just stacked the bin else: # We just stacked a tote if not self._reset: self.tote_count += 1 self._follower.set_goal( Setpoints.TOTE ) # Go back up. After stacking, you should always grab a tote. # If we try to stack a 6th tote it'll break the robot, don't do that. elif (hardware.game_piece_in_intake() or self._manual_stack ) and self.tote_count < 4: # We have something, go down. if not self.has_bin: if self.tote_first or self.tote_count > 0 or self._manual_stack: self._follower.set_goal(Setpoints.BOTTOM) else: # We have a bin, just auto-stack. self._follower.set_goal(Setpoints.BOTTOM) else: # Wait for a game piece & raise the elevator if self.is_empty(): if self.tote_first: self._follower.set_goal(Setpoints.FIRST_TOTE) elif self._cap: setpoint = Setpoints.MAX_TRAVEL - 12 * self.tote_count self._follower.set_goal(setpoint) else: self._follower.set_goal(Setpoints.BIN) else: self._follower.set_goal(Setpoints.TOTE) if self._reset: self._reset = False def reset_encoder(self): hardware.elevator_encoder.reset() self._follower.set_goal(0) self._follower._reset = True def reset_stack(self): self.tote_count = 0 self.has_bin = False self._reset = True def at_goal(self): return self._follower.trajectory_finished( ) # and abs(self._follower.get_goal() - self.position) < 2 def drop_stack(self): self._should_drop = True def stack_tote_first(self): self.tote_first = True def is_full(self): return self.tote_count == 5 and hardware.game_piece_in_intake() def is_empty(self): return self.tote_count == 0 and not self.has_bin def manual_stack(self): self._manual_stack = True def update_nt(self): log.info("position: %s" % hardware.elevator_encoder.getDistance()) log.info("capping? %s" % self._cap) log.info("at goal? %s" % self.at_goal()) log.info("totes: %s" % self.tote_count) def cap(self): self._cap = True def update_follower(self): while True: self._follower.calculate(hardware.elevator_encoder.getDistance()) time.sleep(0.005)
class ClimbSubsystem(Subsystem): def __init__(self, robot): super().__init__("Climb") self.robot = robot self.winchMotor = Spark(robotmap.CLIMBWINCH) self.armUpSolenoid = Solenoid(robotmap.CLIMBARM_UP) self.armDownSolenoid = Solenoid(robotmap.CLIMBARM_DOWN) self.lockCloseSolenoid = Solenoid(robotmap.CLIMBCLAMPLOCK_CLOSE) self.lockOpenSolenoid = Solenoid(robotmap.CLIMBCLAMPLOCK_OPEN) self.hookLimit = DigitalInput(robotmap.LIMIT_SWITCH_HOOK) def pullyclimb(self, speed): self.winchMotor.set(speed) def armExtend(self): self.armUpSolenoid.set(True) self.armDownSolenoid.set(False) def armRetract(self): self.armUpSolenoid.set(False) self.armDownSolenoid.set(True) def clampLock(self): self.lockCloseSolenoid.set(True) self.lockOpenSolenoid.set(False) def clampOpen(self): self.lockCloseSolenoid.set(False) self.lockOpenSolenoid.set(True) def isHookPressed(self): return self.hookLimit.get() == False
class BallChute(): def __init__(self): self.stateTimer = wpilib.Timer() self.stateTimer.start() self.ballRakeExtend = Solenoid(robotmap.PCM2_ID, robotmap.BALL_RAKE_EXTEND_SOLENOID) self.ballRakeRetract = Solenoid(robotmap.PCM2_ID, robotmap.BALL_RAKE_RETRACT_SOLENOID) self.ballHatchExtend = Solenoid(robotmap.PCM2_ID, robotmap.BALL_HATCH_EXTEND_SOLENOID) self.ballHatchRetract = Solenoid(robotmap.PCM2_ID, robotmap.BALL_HATCH_RETRACT_SOLENOID) self.ballTickler = Spark(robotmap.BALL_TICKLER_ID) self.bottomIntake = Spark(robotmap.BOTTOM_INTAKE_ID) self.rakeMotor = Talon(robotmap.RAKE_ID) self.ballRakeExtend.set(False) self.ballRakeRetract.set(True) self.ballHatchExtend.set(False) self.ballHatchRetract.set(True) #-value so motors run that way we want them to def RakeMotorStart(self, value): self.rakeMotor.set(-value) def RakeMotorStop(self): self.rakeMotor.stopMotor() def BottomMotorStart(self, value): self.bottomIntake.set(-value) def BottomMotorStop(self): self.bottomIntake.stopMotor() def BallTicklerStart(self, value): self.ballTickler.set(value) def BallTicklerStop(self): self.ballTickler.stopMotor() #Solenoid functions ot make things easier def DeployRake(self): self.ballRakeExtend.set(True) self.ballRakeRetract.set(False) def StowRake(self): self.ballRakeExtend.set(False) self.ballRakeRetract.set(True) def OpenHatch(self): self.ballHatchExtend.set(True) self.ballHatchRetract.set(False) def CloseHatch(self): self.ballHatchExtend.set(False) self.ballHatchRetract.set(True) def Iterate(self, copilot: XBox): if copilot.Y(): if copilot.Start(): self.BottomMotorStart(1) elif copilot.X(): self.BottomMotorStart(-.5) else: self.BottomMotorStart(.5) else: self.BottomMotorStop() if copilot.A(): elif copilot.X(): self.RakeMotorStart(-.5) else: self.RakeMotorStart(.5) else:
class Elevator(Component): ON_TARGET_DELTA = 1 / 4 def __init__(self): super().__init__() self._motor = SyncGroup(Talon, constants.motor_elevator) self._position_encoder = Encoder(*constants.encoder_elevator) self._photosensor = DigitalInput(constants.photosensor) self._stabilizer_piston = Solenoid(constants.solenoid_dropper) self._position_encoder.setDistancePerPulse( (PITCH_DIAMETER * math.pi) / TICKS_PER_REVOLUTION) # Trajectory controlling stuff self._follower = TrajectoryFollower() self.assure_tote = CircularBuffer(5) self.auto_stacking = True # Do the dew self._tote_count = 0 # Keep track of totes! self._has_bin = False # Do we have a bin on top? self._new_stack = True # starting a new stack? self._tote_first = False # Override bin first to grab totes before anything else self._should_drop = False # Are we currently trying to get a bin ? self._close_stabilizer = True # Opens the stabilizer manually self.force_stack = False # manually actuates the elevator down and up self._follower.set_goal(Setpoints.BIN) # Base state self._follower_thread = Thread(target=self.update_follower) self._follower_thread.start() self._auton = False quickdebug.add_tunables(Setpoints, ["DROP", "STACK", "BIN", "TOTE", "FIRST_TOTE"]) quickdebug.add_printables( self, [('position', self._position_encoder.getDistance), ('photosensor', self._photosensor.get), "has_bin", "tote_count", "tote_first", "at_goal", "has_game_piece", "auto_stacking"]) def stop(self): self._motor.set(0) def update(self): goal = self._follower.get_goal() if self.at_goal: self.assure_tote.append(self.has_game_piece) if self._should_drop: # Overrides everything else if self.tote_count < 5 and not self.has_game_piece: self._follower._max_acc = 100 # Slow down on drop self._follower.set_goal(Setpoints.DROP) self._close_stabilizer = False self._new_stack = True else: self._follower._max_acc = 210 # Normal speed if self._new_stack: self._new_stack = False self._close_stabilizer = True self._tote_count = 0 self._has_bin = False if goal == Setpoints.STACK: # If we've just gone down to grab something if self.tote_count == 0 and not self.has_bin and not self.tote_first: self._has_bin = True # Count the bin else: # We were waiting for a tote self.add_tote() self._follower.set_goal(Setpoints.AUTON if self._auton else Setpoints.TOTE) # Go back up if self._tote_count >= 2: self._close_stabilizer = True elif ( self.tote_in_long_enough() and self.auto_stacking ) and self.tote_count < 5: # If we try to stack a 6th tote it'll break the robot if not self.has_bin: # Without a bin, only stack in these situations: # - We're picking up a tote, and this is the first tote. # - We're forcing the elevator to stack (for the bin, usually) # - We have more than one tote already, so we can pick up more. if self.tote_first or self.force_stack or self.tote_count > 0: self._follower.set_goal(Setpoints.STACK) else: # We have a bin, just auto-stack. self._follower.set_goal(Setpoints.STACK) if self.has_bin: # Transfer! if self._tote_count == 1: self._close_stabilizer = False else: # Wait for a game piece & raise the elevator if self._auton: self._follower.set_goal(Setpoints.AUTON) elif self._tote_count == 0 and not self.has_bin: if self.tote_first: self._follower.set_goal(Setpoints.FIRST_TOTE) else: self._follower.set_goal(Setpoints.BIN) else: self._follower.set_goal(Setpoints.TOTE) self._motor.set(self._follower.output) self._stabilizer_piston.set(self._close_stabilizer) self._should_drop = False self._tote_first = False self._auton = False def reset_encoder(self): self._position_encoder.reset() self._follower.set_goal(0) self._follower._reset = True @property def has_game_piece(self): return not self._photosensor.get() or self.force_stack @property def position(self): return self._position_encoder.getDistance() @property def at_goal(self): return self._follower.trajectory_finished( ) # and abs(self._follower.get_goal() - self.position) < 2 def drop_stack(self): self._should_drop = True def stack_tote_first(self): self._tote_first = True def full(self): return self._tote_count == 5 and self.has_game_piece @property def has_bin(self): return self._has_bin @property def tote_first(self): return self._tote_first @property def tote_count(self): return self._tote_count def add_tote(self, number=1): self._tote_count = max(0, min(5, self._tote_count + number)) def remove_tote(self): self.add_tote(-1) def set_bin(self, bin_): self._has_bin = bin_ def auton(self): self._auton = True self._tote_first = True def tote_in_long_enough(self): return all(self.assure_tote) def update_follower(self): while True: self._follower.calculate(self.position) time.sleep(0.005)