def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
self.launchMin = self.k_launchMin
self.launchMax = self.k_launchMax
self.launchRange = self.launchMax - self.launchMin
self.controlMode = None
self.visionState = robot.visionState
self.intakeLeftMotor = CANTalon(robot.map.k_IlLeftMotorId)
self.intakeLeftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeLeftMotor.reverseSensor(True)
self.intakeLeftMotor.setSafetyEnabled(False)
self.intakeRightMotor = CANTalon(robot.map.k_IlRightMotorId)
self.intakeRightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeRightMotor.setSafetyEnabled(False)
self.aimMotor = CANTalon(robot.map.k_IlAimMotorId)
# LiveWindow.addActuator("IntakeLauncher", "AimMotor", self.aimMotor)
if self.aimMotor.isSensorPresent(CANTalon.FeedbackDevice.AnalogPot):
self.aimMotor.setSafetyEnabled(False)
self.aimMotor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot)
self.aimMotor.enableLimitSwitch(True, True)
self.aimMotor.enableBrakeMode(True)
self.aimMotor.setAllowableClosedLoopErr(5)
self.aimMotor.configPeakOutpuVoltage(12.0, -12.0)
self.aimMotor.setVoltageRampRate(150)
# TODO: setPID if needed
self.boulderSwitch = DigitalInput(robot.map.k_IlBoulderSwitchPort)
self.launcherServoLeft = Servo(robot.map.k_IlServoLeftPort)
self.launcherServoRight = Servo(robot.map.k_IlServoRightPort)
def __init__(self):
super().__init__(name="Climber")
self.trigger = Servo(4)
self.trigger.set(0)
self.climb_victor1 = VictorSP(2)
self.climb_victor2 = VictorSP(3)
self.climb_pdp_port1 = 2
self.climb_pdp_port2 = 3
self.pdp = PowerDistributionPanel()
class Peripherals(Subsystem):
def __init__(self, robot):
super().__init__()
self.intake_spark = Spark(6)
self.control_panel_spark = Spark(5)
self.left_dispenser_gate = Servo(7)
self.right_dispenser_gate = Servo(8)
def run_intake(self, power=0):
self.intake_spark.set(power)
def run_spinner(self, power=0):
self.control_panel_spark.set(power)
def close_gate(self):
self.left_dispenser_gate.setAngle(120)
self.right_dispenser_gate.setAngle(135)
def open_gate(self):
self.left_dispenser_gate.setAngle(0)
self.right_dispenser_gate.setAngle(0)
def panel_clockwise(self, power):
print(power)
self.control_panel_spark.set(power)
def log(self):
pass
class HatchLatch(Subsystem, metaclass=singleton.Singleton):
"""The HatchLatch subsystem controls the latch which grabs hatch panels
and encoders."""
def __init__(self):
super().__init__()
self.servo = Servo(0)
def setOpen(self):
"""Open the hatch latch."""
self.servo.setAngle(Constants.HATCH_LATCH_OPENED)
def setClosed(self):
"""Close the hatch latch."""
self.servo.setAngle(Constants.HATCH_LATCH_CLOSED)
def setToggle(self):
"""Toggle the hatch latch between open and closed
(if it is not in either state, it will be closed)."""
if self.servo.getAngle() == Constants.HATCH_LATCH_CLOSED:
self.setOpen()
elif self.servo.getAngle() == Constants.HATCH_LATCH_OPENED:
self.setClosed()
else:
self.setClosed()
def outputToSmartDashboard(self):
Dash.putNumber("Servo Angle", self.servo.getAngle())
class Gripper(Subsystem):
def __init__(self, robot):
self.robot = robot
self.isOpen = True
self.left_servo = Servo(3)
self.right_servo = Servo(4)
def open(self):
self.left_servo.set(0.0)
self.right_servo.set(1.0)
def close(self):
self.left_servo.set(0.5)
self.right_servo.set(0.5)
def toggle(self):
(self.close, self.open)[self.isOpen]()
class Climber(Subsystem):
def __init__(self):
super().__init__(name="Climber")
self.trigger = Servo(4)
self.trigger.set(0)
self.climb_victor1 = VictorSP(2)
self.climb_victor2 = VictorSP(3)
self.climb_pdp_port1 = 2
self.climb_pdp_port2 = 3
self.pdp = PowerDistributionPanel()
def close_gate(self):
self.trigger.set(0)
def open_gate(self):
self.trigger.set(0.5)
def active_climber(self, climb_power):
self.climb_victor1.set(climb_power)
self.climb_victor2.set(climb_power)
def inactive_climber(self):
self.climb_victor1.set(0)
self.climb_victor2.set(0)
def check_continuous_faults(self):
"""
Check if the system is faulted.
For the climber, we are interested in whether the versa dual-input has failed.
:return:
"""
master_current = self.pdp.getCurrent(self.climb_pdp_port1)
slave_current = self.pdp.getCurrent(self.climb_pdp_port2)
ref = master_current
if ref == 0:
ref = slave_current
if ref == 0:
ref = 1
eps = 1e-1
return False # (master_current > eps or slave_current > eps) and abs(master_current - slave_current) / ref < 1
class HatchLatch(Subsystem, metaclass=singleton.Singleton):
"""The HatchLatch subsystem controls the latch which grabs hatch panels
and encoders."""
def __init__(self):
super().__init__()
self.servo = Servo(0)
def setOpen(self):
"""Open the hatch latch."""
self.servo.setAngle(Constants.HATCH_LATCH_OPENED)
def setClosed(self):
"""Close the hatch latch."""
self.servo.setAngle(Constants.HATCH_LATCH_CLOSED)
def setToggle(self):
"""Toggle the hatch latch between open and closed
(if it is not in either state, it will be closed)."""
if self.servo.getAngle() == Constants.HATCH_LATCH_CLOSED:
self.setOpen()
elif self.servo.getAngle() == Constants.HATCH_LATCH_OPENED:
self.setClosed()
else:
self.setClosed()
def outputToDashboard(self):
Dash.putNumber("Servo Angle", self.servo.getAngle())
def periodic(self):
self.outputToDashboard()
def controlHatch(self):
""" Checks each trigger and opens and closes depending on the trigger pressed."""
if oi.OI.operator_buttons[6].whenPressed():
self.setOpen()
elif oi.OI.operator_buttons[5].whenPressed():
self.setClosed()
def __init__(self, robot):
super().__init__()
self.intake_spark = Spark(6)
self.control_panel_spark = Spark(5)
self.left_dispenser_gate = Servo(7)
self.right_dispenser_gate = Servo(8)
def __init__(self):
super().__init__()
self.servo = Servo(0)
def __init__(self, robot):
self.robot = robot
self.isOpen = True
self.left_servo = Servo(3)
self.right_servo = Servo(4)
class IntakeLauncher(Subsystem):
"""A class to manage/control all aspects of shooting boulders.
IntakeLauncher is comprised of:
aimer: to raise & lower the mechanism for ball intake and shooting
wheels: to suck or push the boulder
launcher: to push boulder into the wheels during shooting
limit switches: to detect if aimer is at an extreme position
potentiometer: to measure our current aim position
boulderSwitch: to detect if boulder is present
Aiming is controlled via two modes
1: driver/interactive: aimMotor runs in VBUS mode to change angle
2: auto/vision control: aimMotor runs in closed-loop position mode
to arrive at a designated position
"""
k_launchMin = 684.0 # manual calibration value
k_launchMinDegrees = -11 # ditto (vestigial)
k_launchMax = 1024.0 # manual calibration value
k_launchMaxDegrees = 45 # ditto (vestigial)
k_launchRange = k_launchMax - k_launchMin
k_launchRangeDegrees = k_launchMaxDegrees - k_launchMinDegrees
k_aimDegreesSlop = 2 # TODO: tune this
k_intakeSpeed = -0.60 # pct vbux
k_launchSpeedHigh = 1.0 # pct vbus
k_launchSpeedLow = 0.7
k_launchSpeedZero = 0
k_servoLeftLaunchPosition = 0.45 # servo units
k_servoRightLaunchPosition = 0.65
k_servoLeftNeutralPosition = 0.75
k_servoRightNeutralPosition = 0.4
k_aimMultiplier = 0.5
k_maxPotentiometerError = 5 # potentiometer units
# launcher positions are measured as a percent of the range
# of the potentiometer..
# intake: an angle approriate for intaking the boulder
# neutral: an angle appropriate for traversing the lowbar
# travel: an angle appropriate for traversing the rockwall, enableLimitSwitch
# highgoal threshold: above which we shoot harder
k_launchIntakeRatio = 0.0
k_launchTravelRatio = 0.51
k_launchNeutralRatio = 0.51
k_launchHighGoalThreshold = 0.69
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
self.launchMin = self.k_launchMin
self.launchMax = self.k_launchMax
self.launchRange = self.launchMax - self.launchMin
self.controlMode = None
self.visionState = robot.visionState
self.intakeLeftMotor = CANTalon(robot.map.k_IlLeftMotorId)
self.intakeLeftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeLeftMotor.reverseSensor(True)
self.intakeLeftMotor.setSafetyEnabled(False)
self.intakeRightMotor = CANTalon(robot.map.k_IlRightMotorId)
self.intakeRightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeRightMotor.setSafetyEnabled(False)
self.aimMotor = CANTalon(robot.map.k_IlAimMotorId)
# LiveWindow.addActuator("IntakeLauncher", "AimMotor", self.aimMotor)
if self.aimMotor.isSensorPresent(CANTalon.FeedbackDevice.AnalogPot):
self.aimMotor.setSafetyEnabled(False)
self.aimMotor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot)
self.aimMotor.enableLimitSwitch(True, True)
self.aimMotor.enableBrakeMode(True)
self.aimMotor.setAllowableClosedLoopErr(5)
self.aimMotor.configPeakOutpuVoltage(12.0, -12.0)
self.aimMotor.setVoltageRampRate(150)
# TODO: setPID if needed
self.boulderSwitch = DigitalInput(robot.map.k_IlBoulderSwitchPort)
self.launcherServoLeft = Servo(robot.map.k_IlServoLeftPort)
self.launcherServoRight = Servo(robot.map.k_IlServoRightPort)
def initDefaultCommand(self):
self.setDefaultCommand(ILCmds.DriverControlCommand(self.robot))
def updateDashboard(self):
pass
def beginDriverControl(self):
self.setControlMode("vbus")
def driverControl(self, deltaX, deltaY):
if self.robot.visionState.wantsControl():
self.trackVision()
else:
self.setControlMode("vbus")
self.aimMotor.set(deltaY * self.k_aimMultiplier)
def endDriverControl(self):
self.aimMotor.disableControl()
# ----------------------------------------------------------------------
def trackVision(self):
if not self.visionState.TargetAcquired:
return # hopefully someone is guiding the drivetrain to help acquire
if not self.visionState.LauncherLockedOnTarget:
if math.fabs(self.visionState.TargetY - self.getAngle()) < self.k_aimDegreesSlop:
self.visionState.LauncherLockedOnTarget = True
else:
self.setPosition(self.degreesToTicks(self.TargetY))
elif self.visionState.DriveLockedOnTarget:
# here we shoot and then leave AutoAimMode
pass
else:
# do nothing... waiting form driveTrain to reach orientation
pass
def setControlMode(self, mode):
if mode != self.controlMode:
self.controlMode = mode
if mode == "vbus":
self.aimMotor.disableControl()
self.aimMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.aimMotor.enableControl()
elif mode == "position":
self.aimMotor.disableControl()
self.aimMotor.changeControlMode(CANTalon.ControlMode.Position)
self.aimMotor.enableControl()
elif mode == "disabled":
self.aimMotor.disableControl()
else:
self.robot.error("ignoring controlmode " + mode)
self.controlMode = None
self.aimMotor.disableControl()
# launcher aim -------------------------------------------------------------
def getPosition(self):
return self.aimMotor.getPosition()
def getAngle(self):
pos = self.getPosition()
return self.ticksToDegress(pos)
def setPosition(self, pos):
self.setControlMode("position")
self.aimMotor.set(pos)
def isLauncherAtTop(self):
return self.aimMotor.isFwdLimitSwitchClosed()
def isLauncherAtBottom(self):
return self.aimMotor.isRevLimitSwitchClosed()
def isLauncherAtIntake(self):
if self.k_intakeRatio == 0.0:
return self.isLauncherAtBottom()
else:
return self.isLauncherNear(self.getIntakePosition())
def isLauncherAtNeutral(self):
return self.isLauncherNear(self.getNeutralPosition())
def isLauncherAtTravel(self):
return self.isLauncherNear(self.getTravelPosition())
def isLauncherNear(self, pos):
return math.fabs(self.getPosition() - pos) < self.k_maxPotentiometerError
def getIntakePosition(self):
return self.getLauncherPositionFromRatio(self.k_launchIntakeRatio)
def getNeutralPosition(self):
return self.getLauncherPositionFromRatio(self.k_launchNeutralRatio)
def getTravelPosition(self):
return self.getLauncherPositionFromRatio(self.k_launchTravelRatio)
def getLauncherPositionFromRatio(self, ratio):
return self.launchMin + ratio * self.launchRange
def degressToTicks(self, deg):
ratio = (deg - self.k_launchMinDegrees) / self.k_launchRangeDegrees
return self.k_launchMin + ratio * self.k_launchRange
def ticksToDegrees(self, t):
ratio = (t - self.k_launchMin) / self.k_launchRange
return self.k_launchMinDegrees + ratio * self.k_launchRangeDegrees
def calibratePotentiometer(self):
if self.isLauncherAtBottom():
self.launchMin = self.getPosition()
elif self.isLauncherAtTop():
self.launchMax = self.getPosition()
self.launchRange = self.launchMax - self.launchMin
# intake wheels controls ---------------------------------------------------
def setWheelSpeedForLaunch(self):
if self.isLauncherAtTop():
speed = self.k_launchSpeedHigh
else:
speed = self.k_launchSpeedLow
self.setSpeed(speed)
def setWheelSpeedForIntake(self):
self.setSpeed(self.k_intakeSpeed)
def stopWheels(self):
self.setSpeed(k_launchSpeedZero)
def setWheelSpeed(self, speed):
self.intakeLeftMotor.set(speed)
self.intakeRightMotor.set(-speed)
# boulder and launcher servo controls --------------------------------------------------
def hasBoulder(self):
return self.boulderSwitch.get()
def activateLauncherServos(self):
self.robot.info("activateLauncherServos at:" + self.getAimPosition())
self.launcherServoLeft.set(self.k_servoLeftLaunchPosition)
self.launcherServoRight.set(self.k_servoRightLaunchPosition)
def retractLauncherServos(self):
self.launcherServoLeft.set(self.k_servoLeftNeutralPosition)
self.launcherServoRight.set(self.k_servoRightNeutralPosition)
def __init__(self):
self.servo = Servo(servoPort)
class ServoSubsystem(Subsystem):
def __init__(self):
self.servo = Servo(servoPort)
def moveServo(self, value):
self.servo.set(value)