class Winch:
def __init__(self, talon_id):
self.talon = TalonSRX(talon_id)
self.talon.configSelectedFeedbackSensor(
TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
self.talon.setQuadraturePosition(0, 0)
self.talon.setInverted(True)
def forward(self):
self.talon.set(TalonSRX.ControlMode.PercentOutput, 0.75)
def reverse(self):
self.talon.set(TalonSRX.ControlMode.PercentOutput, -0.75)
def stop(self):
self.talon.set(TalonSRX.ControlMode.PercentOutput, 0)
def update_smart_dashboard(self):
wpilib.SmartDashboard.putNumber(
"Winch Position", self.talon.getSelectedSensorPosition(0))
wpilib.SmartDashboard.putNumber("Winch Quad Position",
self.talon.getQuadraturePosition())
def setup_talons(self,
master: TalonSRX,
slave: TalonSRX,
invert=False,
pidIdx=0,
timeoutMs=0,
brake=True):
slave.follow(master)
master.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, pidIdx, timeoutMs)
master.enableVoltageCompensation(True)
master.configOpenLoopRamp(1 / 3, timeoutMs)
master.configContinuousCurrentLimit(50, timeoutMs)
master.configPeakCurrentLimit(80, timeoutMs)
master.configPeakCurrentDuration(500, timeoutMs)
neut_mode = TalonSRX.NeutralMode.Brake if brake else TalonSRX.NeutralMode.Coast
master.setNeutralMode(neut_mode)
slave.setNeutralMode(neut_mode)
master.setSensorPhase(False)
master.setInverted(invert)
slave.setInverted(invert)
class Claw:
"""
Implements a finite-state automaton for manipulating the claw.
Parameters:
talon_id: the ID number of the talon on the claw.
contact_sensor_channel: the channel that the contact sensor is
connected to.
"""
claw_movement_time = 0.5 #: time to allow for the claw to open, in seconds
claw_adjust_time = 0.25
def __init__(self, talon_id, follower_id):
"""
Create a new instance of the claw subsystem.
This function constructs and configures the CANTalon instance and the
touch sensor. It also sets the state machine to the neutral starting
state.
"""
self.talon = TalonSRX(talon_id)
self.follower = TalonSRX(follower_id)
self.follower.set(
TalonSRX.ControlMode.Follower,
talon_id
)
self.talon.setInverted(True)
self.follower.setInverted(False)
# Forward limit switch = claw opening limit switch
# self.talon.configForwardLimitSwitchSource(
# TalonSRX.LimitSwitchSource.FeedbackConnector,
# TalonSRX.LimitSwitchNormal.NormallyOpen,
# 0
# )
self.state = 'neutral'
self.closeAdjustTimer = wpilib.Timer()
self.movementTimer = wpilib.Timer()
def set_power(self, power):
self.state = 'manual_ctrl'
self.talon.set(TalonSRX.ControlMode.PercentOutput, power)
def close(self):
"""
Close the claw.
This function does a quick check to make sure it is a valid state
change before actually changing the state to "closing".
For example you cannot set the state to "closing" if the claw is
already gripping the cube.
"""
if self.state != 'closed' and self.state != 'closing':
self.state = 'closing'
self.movementTimer.reset()
self.movementTimer.start()
def open(self):
"""
Open the claw.
The idea of checking the state change is similar to that of the close()
function, but it also resets the opening start time.
"""
if self.state != 'neutral' and self.state != 'opening':
self.state = 'opening'
self.movementTimer.reset()
self.movementTimer.start()
def toggle(self):
"""
Toggle the claw state.
If the claw is closed, open it, and if the claw is opened, close it.
"""
if self.state == 'closed' or self.state == 'closing':
self.open()
elif self.state == 'neutral' or self.state == 'opening':
self.close()
def update(self):
"""
The update function is called periodically and progresses the state
machine.
"""
# if the state is manual control, stop update immediately and return.
if self.state == 'manual_ctrl':
self.movementTimer.reset()
self.movementTimer.stop()
return
# if the state is neutral, clear the start time and set the talon speed
# to 0%
elif self.state == 'neutral':
self.movementTimer.reset()
self.movementTimer.stop()
self.talon.set(TalonSRX.ControlMode.PercentVbus, 0)
# if the state is "closing", set the talon speed to -100%, and if the
# touch sensor is depressed, transition to the "closed" state.
elif self.state == 'closing':
self.talon.set(TalonSRX.ControlMode.PercentVbus, -1.0)
# fwdLim, revLim = self.talon.getLimitSwitchState()
#
# if revLim: # contact sensor pressed?
# self.state = 'closed'
# self.closeAdjustTimer.reset()
# self.closeAdjustTimer.start()
if self.movementTimer.get() > self.claw_movement_time:
self.state = 'closed'
self.closeAdjustTimer.reset()
self.closeAdjustTimer.start()
# if the state is "closed", use less power to the motors--
# Maintain grip, but don't squeeze too hard
elif self.state == 'closed':
if self.closeAdjustTimer.get() > self.claw_adjust_time:
self.talon.set(TalonSRX.ControlMode.PercentVbus, -0.5)
else:
self.talon.set(TalonSRX.ControlMode.PercentVbus, 0)
# if the state is "opening", save the current time if the start time
# is currently empty and set the motor speed to +100%.
# Run until reaching claw_movement_time, then transition to
# the "neutral" state.
elif self.state == 'opening':
self.talon.set(TalonSRX.ControlMode.PercentVbus, 1.0)
if self.movementTimer.get() > self.claw_movement_time:
self.state = 'neutral'
class SwerveModule(object):
def __init__(self, name, steer_id, drive_id):
"""
Performs calculations and bookkeeping for a single swerve module.
Args:
name (str): A NetworkTables-friendly name for this swerve
module. Used for saving and loading configuration data.
steer_id (number): The CAN ID for the Talon SRX controlling this
module's steering.
drive_id (number): The CAN ID for the Talon SRX controlling this
module's driving.
Attributes:
steer_talon (:class:`ctre.cantalon.CANTalon`): The Talon SRX used
to actuate this module's steering.
drive_talon (:class:`ctre.cantalon.CANTalon`): The Talon SRX used
to actuate this module's drive.
steer_target (number): The current target steering position for
this module, in radians.
steer_offset (number): The swerve module's steering zero position.
This value can be determined by manually steering a swerve
module so that it faces forwards relative to the chassis, and
by taking the raw encoder position value (ADC reading); this
value is the steer offset.
drive_reversed (boolean): Whether or not the drive motor's output
is currently reversed.
"""
self.steer_talon = TalonSRX(steer_id)
self.drive_talon = TalonSRX(drive_id)
# Configure steering motors to use abs. encoders
# and closed-loop control
self.steer_talon.selectProfileSlot(0, 0)
self.steer_talon.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0,
0) # noqa: E501
self.steer_talon.configAllowableClosedloopError(
0, math.ceil(_acceptable_steer_err), 0)
self.drive_talon.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder, 0, 0) # noqa: E501
self.drive_talon.setQuadraturePosition(0, 0)
self.drive_talon.configAllowableClosedloopError(
0, math.ceil(_acceptable_drive_err), 0)
self.name = name
self.steer_target = 0
self.steer_target_native = 0
self.drive_temp_flipped = False
self.max_speed = 470 # ticks / 100ms
self.max_observed_speed = 0
self.raw_drive_speeds = []
self.raw_target = 0
self.load_config_values()
def load_config_values(self):
"""
Load saved configuration values for this module via WPILib's
Preferences interface.
The key names are derived from the name passed to the
constructor.
"""
self.steer_talon.selectProfileSlot(0, 0)
preferences = wpilib.Preferences.getInstance()
self.max_speed = preferences.getFloat(self.name + '-Max Wheel Speed',
370)
self.drive_talon.setSensorPhase(
preferences.getBoolean(
self.name + '-Sensor Reverse',
swerve_defaults[self.name]['Sensor Reverse']))
self.steer_talon.setSensorPhase(
preferences.getBoolean(
self.name + '-Steer Sensor Reverse',
swerve_defaults[self.name]['Steer Sensor Reverse']))
self.steer_offset = preferences.getFloat(
self.name + '-offset', swerve_defaults[self.name]['Offset'])
self.steer_min = 0
self.steer_max = 1024
self.steer_range = 1024
self.drive_reversed = preferences.getBoolean(
self.name + '-reversed', swerve_defaults[self.name]['Reversed'])
self.steer_reversed = preferences.getBoolean(
self.name + '-steer-reversed',
swerve_defaults[self.name]['Steer Reversed'])
self.steer_talon.setInverted(self.steer_reversed)
def save_config_values(self):
"""
Save configuration values for this module via WPILib's
Preferences interface.
"""
preferences = wpilib.Preferences.getInstance()
preferences.putFloat(self.name + '-offset', self.steer_offset)
preferences.putBoolean(self.name + '-reversed', self.drive_reversed)
if _apply_range_hack:
preferences.putFloat(self.name + '-min', self.steer_min)
preferences.putFloat(self.name + '-max', self.steer_max)
def get_steer_angle(self):
"""
Get the current angular position of the swerve module in
radians.
"""
native_units = self.steer_talon.getSelectedSensorPosition()
native_units -= self.steer_offset
# Position in rotations
rotation_pos = native_units / self.steer_range
return rotation_pos * 2 * math.pi
def set_steer_angle(self, angle_radians):
"""
Steer the swerve module to the given angle in radians.
`angle_radians` should be within :math:`[-2\\pi, 2\\pi]`.
This method attempts to find the shortest path to the given
steering angle; thus, it may in actuality servo to the
position opposite the passed angle and reverse the drive
direction.
Args:
angle_radians (number): The angle to steer towards in radians,
where 0 points in the chassis forward direction.
"""
n_rotations = math.trunc(
(self.steer_talon.getSelectedSensorPosition(0) - self.steer_offset)
/ self.steer_range)
current_angle = self.steer_talon.getSelectedSensorPosition(0)
current_angle -= self.steer_offset
current_angle *= (math.pi / 512)
adjusted_target = angle_radians + (n_rotations * 2 * math.pi)
possible_angles = [
adjusted_target + math.pi,
adjusted_target - math.pi,
adjusted_target + (2 * math.pi),
adjusted_target - (2 * math.pi),
]
# Shortest-path servoing
should_reverse_drive = False
shortest_tgt = adjusted_target
for i, target in enumerate(possible_angles):
if abs(target - current_angle) < abs(shortest_tgt - current_angle):
shortest_tgt = target
if i == 0 or i == 1:
should_reverse_drive = True
else:
should_reverse_drive = False
self.steer_target = shortest_tgt
self.steer_talon.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0,
0)
# Compute and send actual target to motor controller
native_units = (self.steer_target * 512 / math.pi) + self.steer_offset
self.raw_target = native_units
self.steer_talon.set(ControlMode.Position, native_units)
self.drive_temp_flipped = should_reverse_drive
def set_drive_speed(self, speed, direct=False):
"""
Drive the swerve module wheels at a given percentage of
maximum power or speed.
Args:
percent_speed (number): The speed to drive the module at, expressed
as a percentage of maximum speed. Negative values drive in
reverse.
"""
if self.drive_reversed:
speed *= -1
if self.drive_temp_flipped:
speed *= -1
self.drive_talon.selectProfileSlot(1, 0)
self.drive_talon.config_kF(0, 1023 / self.max_speed, 0)
if direct:
self.drive_talon.set(ControlMode.Velocity, speed)
else:
self.drive_talon.set(ControlMode.Velocity, speed * self.max_speed)
def set_drive_percent_out(self, pct_out):
if self.drive_reversed:
pct_out *= -1
if self.drive_temp_flipped:
pct_out *= -1
self.drive_talon.set(ControlMode.PercentOutput, pct_out)
def set_drive_distance(self, ticks):
if self.drive_reversed:
ticks *= -1
if self.drive_temp_flipped:
ticks *= -1
self.drive_talon.selectProfileSlot(0, 0)
self.drive_talon.set(ControlMode.Position, ticks)
def reset_drive_position(self):
self.drive_talon.setQuadraturePosition(0, 0)
def apply_control_values(self, angle_radians, speed, direct=False):
"""
Set a steering angle and a drive speed simultaneously.
Args:
angle_radians (number): The desired angle to steer towards.
percent_speed (number): The desired percentage speed to drive at.
See Also:
:func:`~set_drive_speed` and :func:`~set_steer_angle`
"""
self.set_steer_angle(angle_radians)
self.set_drive_speed(speed, direct)
def update_smart_dashboard(self):
"""
Push various pieces of info to the Smart Dashboard.
This method calls to NetworkTables (eventually), thus it may
be _slow_.
As of right now, this displays the current raw absolute encoder reading
from the steer Talon, and the current target steer position.
"""
self.raw_drive_speeds.append(self.drive_talon.getQuadratureVelocity())
if len(self.raw_drive_speeds) > 50:
self.raw_drive_speeds = self.raw_drive_speeds[-50:]
self.cur_drive_spd = np.mean(self.raw_drive_speeds)
if abs(self.cur_drive_spd) > abs(self.max_observed_speed):
self.max_observed_speed = self.cur_drive_spd
wpilib.SmartDashboard.putNumber(
self.name + ' CL Position',
self.steer_talon.getSelectedSensorPosition(0))
wpilib.SmartDashboard.putNumber(self.name + ' ADC',
self.steer_talon.getAnalogInRaw())
wpilib.SmartDashboard.putNumber(
self.name + ' Drive Ticks',
self.drive_talon.getQuadraturePosition())
wpilib.SmartDashboard.putNumber(self.name + ' Drive Velocity',
self.cur_drive_spd)
wpilib.SmartDashboard.putNumber(self.name + ' Steer Error',
self.steer_talon.getClosedLoopError(0))
wpilib.SmartDashboard.putNumber(self.name + ' Drive Error',
self.drive_talon.getClosedLoopError(0))
if _enable_debug_dashboard_values:
wpilib.SmartDashboard.putNumber(
self.name + ' Position',
(self.steer_talon.getAnalogIn() - self.steer_offset) *
(180 / 512))
wpilib.SmartDashboard.putNumber(self.name + ' Raw Position',
self.steer_talon.getAnalogIn())
wpilib.SmartDashboard.putNumber(self.name + ' Target',
self.raw_target)
wpilib.SmartDashboard.putNumber(
self.name + ' Drive Velocity (Max)', self.max_observed_speed)
if wpilib.RobotBase.isReal():
if _enable_debug_dashboard_values:
wpilib.SmartDashboard.putNumber(
self.name + ' Drive Percent Output',
self.drive_talon.getMotorOutputPercent())
wpilib.SmartDashboard.putNumber(
self.name + ' Drive Current',
self.drive_talon.getOutputCurrent())
wpilib.SmartDashboard.putNumber(
self.name + ' Steer Current',
self.steer_talon.getOutputCurrent())
class Elevator(FaultableSystem):
def __init__(self, mock=False):
super().__init__("Elevator")
self.talon_master = TalonSRX(4)
self.talon_slave = TalonSRX(5)
self._state = ElevatorState.HOLDING
self.nlogger = Logger("elevator")
self.nlogger.add("Time", robot_time.delta_time)
self.nlogger.add("Position", self.get_elevator_position)
self.nlogger.add("Voltage", self.talon_master.getMotorOutputVoltage)
self.nlogger.add("Current", self.talon_master.getOutputCurrent)
# self.nlogger.start()
dashboard2.add_graph("Elevator Position", self.get_elevator_position)
dashboard2.add_graph("Elevator Voltage", self.talon_master.getMotorOutputVoltage)
dashboard2.add_graph("Elevator Current", self.talon_master.getOutputCurrent)
dashboard2.add_graph("Elevator Current2", self.talon_slave.getOutputCurrent)
dashboard2.add_graph("Elevator State", lambda: self._state)
if not mock:
self.mp_manager = SRXMotionProfileManager(self.talon_master, 1000 // FREQUENCY)
self.talon_master.setQuadraturePosition(0, 0)
self.talon_slave.follow(self.talon_master)
# 1023 units per 12V
# This lets us pass in feedforward as voltage
self.talon_master.config_kF(MAIN_IDX, 1023/12, 0)
self.talon_master.config_kF(EXTENT_IDX, 1023/12, 0)
kP = 0.05 * 1023 / 4096
# self.talon_master.config_kP(MAIN_IDX, kP, 0)
# self.talon_master.config_kP(EXTENT_IDX, kP, 0)
self.talon_master.config_kP(HOLD_MAIN_IDX, .1 * 1023 / 4096, 0)
self.talon_master.config_kP(HOLD_EXTENT_IDX, .3 * 1023 / 4096, 0)
self.talon_master.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.talon_master.setSensorPhase(True)
invert = False
self.talon_master.setInverted(invert)
self.talon_slave.setInverted(invert)
def init_profile(self, new_pos):
if self._state != ElevatorState.HOLDING:
return False
profile, _ = self.gen_profile(self.get_elevator_position(), new_pos)
self.mp_manager.init_profile(profile)
self.talon_master.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
return True
def start_profile(self):
self._state = ElevatorState.MOVING
return self.mp_manager.start_profile()
def has_finished_profile(self):
return self.mp_manager.is_done()
def finish_profile(self):
self._state = ElevatorState.HOLDING
self.hold()
def set_power(self, power):
self._state = ElevatorState.MANUAL
self.talon_master.set(ControlMode.PercentOutput, power)
def get_mass(self, pos):
"""
Mass in lbm
:param pos:
:return:
"""
if pos <= CARRIAGE_TRAVEL:
return CARRIAGE_WEIGHT
return CARRIAGE_WEIGHT + EXTENT_WEIGHT
def get_hold_torque(self, pos):
"""
Torque in lb-in
:param pos:
:return:
"""
return self.get_mass(pos) * SPOOL_RADIUS
def get_pid_index(self, pos):
if pos <= CARRIAGE_TRAVEL:
return MAIN_IDX
return EXTENT_IDX
def calc_ff(self, pos, vel, acc):
"""
Returns feedforward in volts
:param pos: Position of the elevator, in inches
:param vel: Desired velocity, in in/s
:param acc: Desired acceleration, in in/s^2
:return: Feedforward voltage
"""
hold_voltage = 12 * self.get_hold_torque(pos) / self.get_stall_torque()
vel_maxv = 12 - hold_voltage
vel_voltage = vel_maxv * vel / (self.get_max_speed() * (vel_maxv / 12))
acc_maxv = 12 - hold_voltage - vel_voltage
acc_voltage = 0
return hold_voltage + vel_voltage + acc_voltage
def gen_profile(self, start, end) -> Tuple[List[TalonPoint], int]:
# if not 0 <= end <= TOP_EXTENT:
# raise ValueError(f"End must be within 0 and {TOP_EXTENT}")
talon_points = []
rawmp = MotionProfile(start=start, end=end, cruise_speed=CRUISE_SPEED, acc=ACC, frequency=FREQUENCY)
for point in rawmp:
last = point == rawmp[-1]
talonpt = TalonPoint(position=-self.in_to_native_units(point.position),
velocity=self.calc_ff(point.position, point.velocity, point.acc),
headingDeg=0,
profileSlotSelect0=self.get_pid_index(point.position),
profileSlotSelect1=0,
isLastPoint=last,
zeroPos=False,
timeDur=0)
talon_points.append(talonpt)
return talon_points, FREQUENCY
def get_elevator_position(self):
"""
:return: The elevator's position as measured by the encoder, in inches. 0 is bottom, 70 is top
"""
return -self.native_to_inches(self.talon_master.getQuadraturePosition())
def get_stall_torque(self):
"""
Motor stall torque (N-m) * # of motors * gear ratio * 8.851 lb-in/N-m
:return: 12V stall torque in lb-in
"""
return 0.71 * 2 * GEAR_RATIO * 8.851
def get_max_speed(self):
"""
Motor free speed (rpm) * (1/60) seconds/minute / gear ratio
2*pi*r * RPS = IPS
:return: 12V speed of the elevator in in/s
"""
return 2*math.pi*SPOOL_RADIUS*(18730 * (1/60) / GEAR_RATIO)
def in_to_native_units(self, inches):
return (inches / (2*math.pi*SPOOL_RADIUS)) * 4096 / TRAVEL_RATIO
def native_to_inches(self, native_distance):
return (native_distance / 4096) * (2*math.pi*SPOOL_RADIUS) * TRAVEL_RATIO
def start_zero_position(self):
self.talon_master.selectProfileSlot(MAIN_IDX, 0)
self.talon_master.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 0)
self.talon_master.set(ControlMode.Position, ZERO_POS)
self._state = ElevatorState.ZEROING
def is_done_zeroing(self):
return self._state == ElevatorState.ZEROING and self.talon_master.getClosedLoopError(0) < ZERO_MAX_ERR
def finish_zero_position(self):
self._state = ElevatorState.HOLDING
self.talon_master.setQuadraturePosition(0, 0)
def hold(self):
pos = self.get_elevator_position()
target = self.in_to_native_units(pos)
pid = self.get_pid_index(pos) + 2
self.talon_master.selectProfileSlot(pid, 0)
ff = (1023/12) * self.calc_ff(pos, 0, 0)
if target == 0:
ff = 0
else:
ff /= target
self.talon_master.config_kF(pid, ff, 0)
self.talon_master.set(ControlMode.Position, target)
self._state = ElevatorState.HOLDING
def move_to(self, target):
pos = self.get_elevator_position()
target_n = self.in_to_native_units(target)
pid = self.get_pid_index(pos) + 2
self.talon_master.selectProfileSlot(pid, 0)
ff = (1023/12) * self.calc_ff(pos, 0, 0)
if target_n == 0:
ff = 0
else:
ff /= target_n
self.talon_master.config_kF(pid, ff, 0)
self.talon_master.set(ControlMode.Position, target_n)
self._state = ElevatorState.HOLDING
def is_at_top(self):
return self.talon_master.isFwdLimitSwitchClosed()
def is_at_bottom(self):
return self.talon_master.isRevLimitSwitchClosed()
def check_continuous_faults(self):
"""
Check if the system is faulted.
For the elevator, we are interested in whether the versa dual-input has failed.
:return:
"""
master_current = self.talon_master.getOutputCurrent()
slave_current = self.talon_slave.getOutputCurrent()
ref = master_current
if ref == 0:
ref = slave_current
if ref == 0:
ref = 1
eps = 1e-1
return (master_current > eps or slave_current > eps) and abs(master_current - slave_current) / ref < 1
class ManualControlLift:
def __init__(
self,
main_lift_id, follower_id,
bottom_limit_channel, start_lim_channel
):
self.main_lift_id = main_lift_id
self.follower_id = follower_id
self.lift_main = TalonSRX(main_lift_id)
self.lift_follower = TalonSRX(follower_id)
self.lift_main.configSelectedFeedbackSensor(
TalonSRX.FeedbackDevice.PulseWidthEncodedPosition,
0, 0
)
# self.lift_main.setPulseWidthPosition(0, 0)
self.lift_follower.configSelectedFeedbackSensor(
TalonSRX.FeedbackDevice.PulseWidthEncodedPosition,
0, 0
)
# self.lift_follower.setPulseWidthPosition(0, 0)
self.lift_follower.set(
TalonSRX.ControlMode.Follower,
main_lift_id
)
self.lift_follower.setInverted(True)
self.lift_timer = wpilib.Timer()
self.timer_started = False
self.lift_stop_timer = wpilib.Timer()
self.lift_stop_timer_started = False
self.lift_zero_found = False
self.bottom_limit_switch = wpilib.DigitalInput(bottom_limit_channel)
self.start_limit_switch = wpilib.DigitalInput(start_lim_channel)
self.sustain = -0.08
def load_config_values(self):
prefs = wpilib.Preferences.getInstance()
phase = prefs.getBoolean("Lift: Invert Sensor Phase", True)
self.sustain = prefs.getFloat("Lift: Idle Sustain", -0.08)
self.upper_limit = prefs.getInt("Lift: Upper Limit", None)
self.limits_enabled = prefs.getBoolean("Lift: Limits Enabled", False)
if self.limits_enabled:
# Note: positive / forward power to the motors = lift moves down
# negative / reverse power to the motors = lift moves up
if self.upper_limit is not None:
self.lift_main.configReverseSoftLimitThreshold(
int(self.upper_limit), 0
)
self.lift_main.configReverseSoftLimitEnable(True, 0)
else:
self.lift_main.configReverseSoftLimitEnable(False, 0)
self.lift_main.setSensorPhase(phase)
self.lift_follower.setSensorPhase(phase)
def set_soft_limit_status(self, status):
if self.upper_limit is not None:
self.lift_main.configReverseSoftLimitEnable(status, 0)
else:
self.lift_main.configReverseSoftLimitEnable(False, 0)
def update_smart_dashboard(self):
wpilib.SmartDashboard.putNumber(
"Lift Main Position",
self.lift_main.getSelectedSensorPosition(0)
)
wpilib.SmartDashboard.putNumber(
"Lift Follower Position",
self.lift_follower.getSelectedSensorPosition(0)
)
wpilib.SmartDashboard.putBoolean(
"Lift Bottom Limit Switch",
not self.bottom_limit_switch.get()
)
wpilib.SmartDashboard.putBoolean(
"Lift Found Zero",
self.lift_zero_found
)
wpilib.SmartDashboard.putBoolean(
"Lift Start Position Switch",
not self.start_limit_switch.get()
)
def moveTimed(self, time, power):
if not self.timer_started:
self.timer_started = True
self.lift_timer.reset()
self.lift_timer.start()
elif self.lift_timer.get() < time:
self.lift_main.set(TalonSRX.ControlMode.PercentOutput, power)
else:
self.lift_main.set(TalonSRX.ControlMode.PercentOutput, 0)
def setLiftPower(self, power):
if power > 0 and not self.bottom_limit_switch.get():
self.lift_main.set(TalonSRX.ControlMode.PercentOutput, 0)
else:
self.lift_main.set(TalonSRX.ControlMode.PercentOutput, power)
def checkLimitSwitch(self):
self.set_soft_limit_status(self.lift_zero_found)
if not self.bottom_limit_switch.get():
self.lift_zero_found = True
self.lift_main.setPulseWidthPosition(0, 0)
self.lift_follower.setPulseWidthPosition(0, 0)
self.lift_main.setQuadraturePosition(0, 0)
self.lift_follower.setQuadraturePosition(0, 0)
def driveToStartingPosition(self):
if self.start_limit_switch.get():
self.lift_main.set(TalonSRX.ControlMode.PercentOutput, -0.25)
else:
self.lift_main.set(TalonSRX.ControlMode.PercentOutput, 0)