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):
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 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 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 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())
