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)
