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