class LifterComponent(Events):
class EVENTS:
on_control_move = "on_control_move"
on_manual_move = "on_manual move"
# RPM Multipliers
# ELEVATOR_MULTIPLIER = 1635.15 / 6317.67
ELEVATOR_MULTIPLIER = 2102.35 / 3631.33
CARRIAGE_MULTIPLIER = 1.0 - ELEVATOR_MULTIPLIER
# Max heights of each stage.
ELEVATOR_MAX_HEIGHT = 40
CARRIAGE_MAX_HEIGHT = 40
# Conversion factors (counts to inches)
# CHANGE THESE VALUES
ELEVATOR_CONV_FACTOR = 0.00134
CARRIAGE_CONV_FACTOR = 0.000977
el_down = -1
el_up = 1
carriage_down = -1
carriage_up = 1
MAX_SPEED = 0.5
ELEVATOR_ZERO = 0.0
CARRIAGE_ZERO = 0.0
TIMEOUT_MS = 0
ELEVATOR_kF = 0.0
ELEVATOR_kP = 0.1
ELEVATOR_kI = 0.0
ELEVATOR_kD = 0.0
# ALLOWABLE_ERROR = 2
CARRIAGE_ALLOWABLE_ERROR = int(2 / CARRIAGE_CONV_FACTOR)
ELEVATOR_ALLOWABLE_ERROR = int(2 / ELEVATOR_CONV_FACTOR)
# positions = {
# "floor": 2.0,
# "portal": 34.0,
# "scale_low": 48.0,
# "scale_mid": 60.0,
# "scale_high": 72.0,
# "max_height": 84.0
# }
positions = {
"floor": 0.5,
"portal": 34.0,
"scale_low": 52.0,
"scale_mid": 68.0,
"scale_high": 78.0
}
def __init__(self):
Events.__init__(self)
self.elevator_motor = WPI_TalonSRX(5)
self.elevator_bottom_switch = DigitalInput(9)
self.carriage_motor = WPI_TalonSRX(3)
self.carriage_bottom_switch = DigitalInput(1)
self.carriage_top_switch = DigitalInput(2)
self._create_events([
LifterComponent.EVENTS.on_control_move,
LifterComponent.EVENTS.on_manual_move
])
self._is_reset = False
# configure elevator motor and encoder
self.elevator_motor.setNeutralMode(NeutralMode.Brake)
self.elevator_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSensorPhase(True)
self.elevator_motor.setInverted(True)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configAllowableClosedloopError(
0, LifterComponent.ELEVATOR_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configForwardSoftLimitThreshold(
int(LifterComponent.ELEVATOR_MAX_HEIGHT /
LifterComponent.ELEVATOR_CONV_FACTOR), 0)
self.elevator_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.elevator_motor.setCurrentLimit()
# self.elevator_motor.EnableCurrentLimit()
# self.elevator_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
# configure carriage motor and encoder
self.carriage_motor.setNeutralMode(NeutralMode.Brake)
self.carriage_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.setSensorPhase(True)
self.carriage_motor.setInverted(True)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configAllowableClosedloopError(
0, LifterComponent.CARRIAGE_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configForwardSoftLimitThreshold(
int(LifterComponent.CARRIAGE_MAX_HEIGHT /
LifterComponent.CARRIAGE_CONV_FACTOR), 0)
self.carriage_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.carriage_motor.setCurrentLimit()
# self.carriage_motor.EnableCurrentLimit()
# self.carriage_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
def set_elevator_speed(self, speed):
if (speed > 0 and self.current_elevator_position >= LifterComponent.ELEVATOR_MAX_HEIGHT - 2) \
or (speed < 0 and self.elevator_bottom_switch.get()):
self.elevator_motor.set(0)
else:
self.elevator_motor.set(speed)
self.trigger_event(LifterComponent.EVENTS.on_manual_move)
def set_carriage_speed(self, speed):
if (speed > 0 and self.carriage_top_switch.get()) \
or (speed < 0 and self.carriage_bottom_switch.get()):
self.carriage_motor.set(0)
else:
self.carriage_motor.set(speed)
self.trigger_event(LifterComponent.EVENTS.on_manual_move)
def reset_sensors(self):
self.carriage_motor.setSelectedSensorPosition(
0, 0, LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSelectedSensorPosition(
0, 0, LifterComponent.TIMEOUT_MS)
self._is_reset = True
@property
def current_elevator_position(self) -> float:
return self.elevator_motor.getSelectedSensorPosition(
0) * LifterComponent.ELEVATOR_CONV_FACTOR
@property
def current_carriage_position(self) -> float:
return self.carriage_motor.getSelectedSensorPosition(
0) * LifterComponent.CARRIAGE_CONV_FACTOR
@property
def current_position(self) -> float:
return self.current_elevator_position + self.current_carriage_position
def stop_lift(self):
self.elevator_motor.stopMotor()
self.carriage_motor.stopMotor()
def elevator_to_target_position(self, inches: float):
if self._is_reset:
self.elevator_motor.set(
WPI_TalonSRX.ControlMode.Position,
inches / LifterComponent.ELEVATOR_CONV_FACTOR)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def carriage_to_target_position(self, inches: float):
if self._is_reset:
self.carriage_motor.set(
WPI_TalonSRX.ControlMode.Position,
inches / LifterComponent.CARRIAGE_CONV_FACTOR)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def lift_to_distance(self, inches):
i = inches + 6
elevator = min(i * LifterComponent.ELEVATOR_MULTIPLIER,
LifterComponent.ELEVATOR_MAX_HEIGHT)
carriage = i - elevator
print("lift_to_distance carriage" + str(carriage))
print("lift_to_distance elevate" + str(elevator))
print("lift_to_distance lifter" + str(carriage + elevator))
self.elevator_to_target_position(elevator)
self.carriage_to_target_position(carriage)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def is_at_position(self, position: str) -> bool:
return self.is_at_distance(LifterComponent.positions[position])
def is_at_distance(self, inches: float) -> bool:
return inches - 5 < self.current_position < inches + 5
def closest_position(self) -> tuple:
# returns the key for the position closest to the current position
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
return min(
positions,
key=lambda position: abs(self.current_position - position[1]))
def next_position(self) -> str:
position = self.closest_position()
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
index = positions.index(position)
if index == len(positions) - 1:
return position[0]
return positions[index + 1][0]
def prev_position(self) -> str:
position = self.closest_position()
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
index = positions.index(position)
if index == 0:
return position[0]
return positions[index - 1][0]
class Robot(wpilib.IterativeRobot):
#: Which PID slot to pull gains from. Starting 2018, you can choose from
#: 0,1,2 or 3. Only the first two (0,1) are visible in web-based
#: configuration.
kSlotIdx = 0
#: Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For
#: now we just want the primary one.
kPIDLoopIdx = 0
#: set to zero to skip waiting for confirmation, set to nonzero to wait and
#: report to DS if action fails.
kTimeoutMs = 10
def robotInit(self):
self.talon = WPI_TalonSRX(3)
self.joy = wpilib.Joystick(0)
self.loops = 0
self.lastButton1 = False
self.targetPos = 0
# choose the sensor and sensor direction
self.talon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# choose to ensure sensor is positive when output is positive
self.talon.setSensorPhase(True)
# choose based on what direction you want forward/positive to be.
# This does not affect sensor phase.
self.talon.setInverted(False)
# set the peak and nominal outputs, 12V means full
self.talon.configNominalOutputForward(0, self.kTimeoutMs)
self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
self.talon.configPeakOutputForward(1, self.kTimeoutMs)
self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
# Set the allowable closed-loop error, Closed-Loop output will be
# neutral within this range. See Table in Section 17.2.1 for native
# units per rotation.
self.talon.configAllowableClosedloopError(0, self.kPIDLoopIdx,
self.kTimeoutMs)
# set closed loop gains in slot0, typically kF stays zero - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0, self.kTimeoutMs)
self.talon.config_kP(0, 0.1, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, self.kTimeoutMs)
# zero the sensor
self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx,
self.kTimeoutMs)
def teleopPeriodic(self):
"""
This function is called periodically during operator control
"""
# get gamepad axis - forward stick is positive
leftYstick = self.joy.getY()
# calculate the percent motor output
motorOutput = self.talon.getMotorOutputPercent()
button1 = self.joy.getRawButton(1)
button2 = self.joy.getRawButton(2)
# deadband gamepad
if abs(leftYstick) < 0.1:
leftYstick = 0
# prepare line to print
sb = []
sb.append("\tOut%%: %.3f" % motorOutput)
sb.append("\tPos: %.3fu" %
self.talon.getSelectedSensorPosition(self.kPIDLoopIdx))
if self.lastButton1 and button1:
# Position mode - button just pressed
# 10 Rotations * 4096 u/rev in either direction
self.targetPos = leftYstick * 4096 * 10.0
self.talon.set(WPI_TalonSRX.ControlMode.Position, self.targetPos)
# on button2 just straight drive
if button2:
# Percent voltage mode
self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftYstick)
if self.talon.getControlMode() == WPI_TalonSRX.ControlMode.Position:
# append more signals to print when in speed mode.
sb.append("\terr: %s" %
self.talon.getClosedLoopError(self.kPIDLoopIdx))
sb.append("\ttrg: %.3f" % self.targetPos)
# periodically print to console
self.loops += 1
if self.loops >= 10:
self.loops = 0
print(" ".join(sb))
# save button state for on press detect
self.lastButton1 = button1
