class WristSubsystem(Subsystem):
def __init__(self):
self.encoderUnit = 4096
self.gearRatio = 183.33333333
self.lowerAngleLimit = -10
self.topAngleLimit = 170
self.wristPower = 0
self.wristMotor = TalonSRX(wristMotor)
self.wristMotor.configSelectedFeedbackSensor(
wristMotor.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.wristMotor.setSensorPhase(False)
#def initDefaultCommand(self):
# self.setDefaultCommand(AnalogWristCommand())
def getWristDistanceTicks(self):
return self.wristMotor.getSelectedSensorPosition()
def getRotationAngle(self):
return (
(self.wristMotor.getSelectedSensorPosition() / self.encoderUnit) *
(1 / self.gearRatio) * 360)
#def isWristTopLimit(self):
#def isWristBottomLimit(self):
def setWristPower(self, power):
if getRotationAngle() >= self.topAngleLimit or getRotationAngle(
) <= self.lowerAngleLimit:
wristPower = 0
else:
wristPower = power
def updateOutputs(self):
self.wristMotor.set(BaseMotorController.ControlMode.PercentOutput,
self.wristPower * wristSpeedMultiplier)
class Winch:
def __init__(self, talon_id):
self.talon = TalonSRX(talon_id)
def forward(self):
self.talon.set(TalonSRX.ControlMode.PercentOutput,
.5) #NEED TO FINND VALUE
def reverse(self):
self.talon.set(TalonSRX.ControlMode.PercentOutput,
-.5) #NEED TO FINND VALUE
def stop(self):
self.talon.set(TalonSRX.ControlMode.PercentOutput,
0) #NEED TO FINND VALUE
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())
class RD4BLift:
"""
This class contains helper functions for manipulating the position of the
RD4B lift.
Parameters:
left_id: the ID number of the left talon.
right_id: the ID number of the right talon.
"""
#: Length for one segment of the arm. As of the time this comment
#: was written a segment is 30 inches long.
ARM_LENGTH = 30
def __init__(self, left_id, right_id):
"""
Create a new instance of the RD4B lift.
Calling this constructor initializes the two drive motors and
configures their control modes, and also loads the configuration
values from preferences.
"""
# Create new instances of CANTalon for the left and right motors.
self.left_motor = TalonSRX(left_id)
self.right_motor = TalonSRX(right_id)
# The right motor will follow the left motor, so we will configure
# the left motor as Position control mode, and have the right motor
# follow it.
self.left_motor.configSelectedFeedbackSensor(
TalonSRX.FeedbackDevice.Analog, 0, 0)
self.left_motor.selectProfileSlot(0, 0)
self.right_motor.set(TalonSRX.ControlMode.Follower, left_id)
# finally, load the configuration values.
self.load_config_values()
def load_config_values(self):
"""
Internal helper function for configuration values.
This function loads/reloads configuration values from Preferences.
The necessary keys that need to be defined are:
- "lift potentiometer horizontal angle"
- "lift potentiometer base angle"
- "lift limit up"
This function also precalculates values that are used throughout the
code and sets soft limits for the motor based on encoder values.
"""
# get the preferences instance.
preferences = wpilib.Preferences.getInstance()
# get the encoder value when the bars are horizontal and form right
# angles. This is used for calculations.
self.HORIZONTAL_ANGLE = preferences.getFloat(
"lift potentiometer horizontal angle", 0) # noqa: E501
# get the encoder value when the RD4B is in the fully down position.
self.initial_angle = preferences.getFloat(
"lift potentiometer base angle", 0) # noqa: E501
# pre-convert the initial angle to radians and take the sin of the
# angle.
self.INITIAL_ANGLE_RADIANS_SIN = math.sin(
(self.initial_angle - self.HORIZONTAL_ANGLE) / 512 * math.pi)
# get the upper limit of the encoder, or the encoder value when the
# RD4B is fully extended upward.
self.LIMIT_UP = preferences.getFloat("lift limit up", 0)
# set the soft limits to LIMIT_UP and the initial angle. the motors
# should not go outside of this range.
self.left_motor.configForwardSoftLimitThreshold(self.LIMIT_UP, 0)
self.left_motor.configReverseSoftLimitThreshold(self.initial_angle, 0)
def fully_extend(self):
"""
Fully extend the RD4B upward.
This function runs the motor to the LIMIT_UP position.
"""
self.left_motor.set(TalonSRX.ControlMode.Position, self.LIMIT_UP)
def fully_retract(self):
"""
Fully retract the RD4B to the lowest position.
This function runs the motor to the initial_angle position.
"""
self.left_motor.set(TalonSRX.ControlMode.Position, self.initial_angle)
def set_height(self, inches):
"""
Set the height for the RD4B lift.
Given a height to move to in inches, this function will calculate the
angle the motor needs to run to and apply it. The angle is calculated
by this equation:
.. math::
\\theta _f = \\sin^{-1}(\\frac{\\Delta H}{2 l_{arm}}
+ \\sin(\\theta_i))
Args:
inches: the height in inches to move the RD4B to.
"""
# calculate the final angle as per the equation given above.
final_angle_radians = math.asin(inches / (2 * self.ARM_LENGTH) +
self.INITIAL_ANGLE_RADIANS_SIN)
# convert this final angle from radians to native units.
native_units = ((final_angle_radians * 512 / math.pi) +
self.HORIZONTAL_ANGLE)
# set the left motor to run to this position (the right motor will
# follow it)
self.left_motor.set(TalonSRX.ControlMode.Position, native_units)
def getHeight(self):
"""
Get the height of the RD4B.
This function will get the angle of the RD4B from the encoder and
convert this to a height given this equation:
.. math::
\\Delta H = 2 l_{arm} (\\sin(\\theta_f) - \\sin(\\theta_i))
Returns:
The height of the RD4B at the time this function is called.
"""
# get the final angle from the encoder, and convert to radians.
final_angle_radians = ((self.left_motor.getSelectedSensorPosition(0) -
self.HORIZONTAL_ANGLE) # noqa: E501
* 512 * math.pi)
# calculate the height of the RD4B using the final angle and based on
# the given equation.
height = 2 * self.ARM_LENGTH * (math.sin(final_angle_radians) -
self.INITIAL_ANGLE_RADIANS_SIN)
return height
def isMovementFinished(self):
"""
Check if the RD4B is in motion.
This function gets the closed loop error, which will be very small if
the motor is not currently in motion.
Returns:
True if the RD4B is not in motion
False if the RD4B is currently still in motion.
"""
return self.left_motor.getClosedLoopError(0) < 10
def stop(self):
"""
Stop the RD4B, no matter what position it is in currently.
This can be used as an emergency stop in the case of an encoder
breaking, but it can also be used for general purposes if needed.
"""
# change the control mode to percent vbus and then set the speed to 0.
self.left_motor.set(TalonSRX.ControlMode.PercentVBus, 0)
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_open_time = 0.5 #: time to allow for the claw to open, in seconds
claw_adjust_time = 0.25
def __init__(self, talon_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)
# 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.openTimer = wpilib.Timer()
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'
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.openTimer.reset()
self.openTimer.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.openTimer.reset()
self.openTimer.stop()
return
# if the state is neutral, clear the start time and set the talon speed
# to 0%
elif self.state == 'neutral':
self.openTimer.reset()
self.openTimer.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 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_open_time, then transition to the "neutral"
# state.
elif self.state == 'opening':
self.talon.set(TalonSRX.ControlMode.PercentVbus, 1.0)
if self.openTimer.get() < self.claw_open_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.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0,
0) # noqa: E501
self.steer_talon.selectProfileSlot(0, 0)
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.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.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)
sensor_phase = preferences.getBoolean(self.name + '-Sensor Reverse',
False)
self.drive_talon.setSensorPhase(sensor_phase)
self.steer_offset = preferences.getFloat(self.name + '-offset', 0)
if _apply_range_hack:
self.steer_min = preferences.getFloat(self.name + '-min', 0)
self.steer_max = preferences.getFloat(self.name + '-max', 1024)
actual_steer_range = int(self.steer_max - self.steer_min)
self.steer_min += int(actual_steer_range * 0.01)
self.steer_max -= int(actual_steer_range * 0.01)
self.steer_offset -= self.steer_min
self.steer_range = int(self.steer_max - self.steer_min)
else:
self.steer_min = 0
self.steer_max = 1024
self.steer_range = 1024
self.drive_reversed = preferences.getBoolean(self.name + '-reversed',
False)
self.steer_reversed = preferences.getBoolean(
self.name + '-steer-reversed', False)
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(0)
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.get_steer_angle()
adjusted_target = angle_radians + (n_rotations * 2 * math.pi)
# Perform angle unwrapping for target angles.
# This prevents issues resulting from discontinuities in input angle
# ranges.
if abs(adjusted_target - current_angle) - math.pi > 0.0005:
if adjusted_target > current_angle:
adjusted_target -= 2 * math.pi
else:
adjusted_target += 2 * math.pi
# Shortest-path servoing
should_reverse_drive = False
if abs(adjusted_target - current_angle) - (math.pi / 2) > 0.0005:
# shortest path is to move to opposite angle and reverse drive dir
if adjusted_target > current_angle:
adjusted_target -= math.pi
else: # angle_radians < local_angle
adjusted_target += math.pi
should_reverse_drive = True
self.steer_target = adjusted_target
# Compute and send actual target to motor controller
native_units = (self.steer_target * 512 / math.pi) + self.steer_offset
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_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.
"""
wpilib.SmartDashboard.putNumber(
self.name + ' Position',
(self.steer_talon.getSelectedSensorPosition(0) - self.steer_offset)
* 180 / 512)
wpilib.SmartDashboard.putNumber(self.name + ' ADC',
self.steer_talon.getAnalogInRaw())
wpilib.SmartDashboard.putNumber(self.name + ' Target',
self.steer_target * 180 / math.pi)
wpilib.SmartDashboard.putNumber(self.name + ' Steer Error',
self.steer_talon.getClosedLoopError(0))
wpilib.SmartDashboard.putNumber(self.name + ' Drive Error',
self.drive_talon.getClosedLoopError(0))
wpilib.SmartDashboard.putNumber(
self.name + ' Drive Ticks',
self.drive_talon.getQuadraturePosition())
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 + ' Drive Velocity',
self.cur_drive_spd)
wpilib.SmartDashboard.putNumber(self.name + ' Drive Velocity (Max)',
self.max_observed_speed)
if wpilib.RobotBase.isReal():
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 Robot(wpilib.IterativeRobot):
# Lift talon IDs:
main_lift_id = 31
follower_id = 42
# Lift of one stage of the RD4B, in inches
ARM_LENGTH = 28
controller_index = 1
control_axis_index = 2
def __load_config(self):
self.max_height = self.__prefs.getFloat("Lift Max Height", 96)
# get the encoder value when the bars are horizontal and form right
# angles. This is used for calculations.
self.HORIZONTAL_ANGLE = self.__prefs.getFloat(
"Lift Pot Horizontal Position", 0) # noqa: E501
# get the encoder value when the RD4B is in the fully down position.
self.initial_angle = self.__prefs.getFloat("Lift Pot Lower Limit",
0) # noqa: E501
# pre-convert the initial angle to radians and take the sin of the
# angle.
self.INITIAL_ANGLE_RADIANS_SIN = math.sin(
(self.initial_angle - self.HORIZONTAL_ANGLE) * angle_conv_factor)
# get the upper limit of the encoder, or the encoder value when the
# RD4B is fully extended upward.
self.LIMIT_UP = self.__prefs.getFloat("Lift Pot Upper Limit", 0)
# set the soft limits to LIMIT_UP and the initial angle. the motors
# should not go outside of this range.
self.lift_main.configForwardSoftLimitThreshold(int(self.LIMIT_UP), 0)
self.lift_main.configReverseSoftLimitThreshold(int(self.initial_angle),
0)
self.lift_main.setSensorPhase(
self.__prefs.getBoolean("Lift Sensor Phase", False))
def __update_smart_dashboard(self):
final_angle_radians = ((self.lift_main.getSelectedSensorPosition(0) -
self.HORIZONTAL_ANGLE) # noqa: E501
* angle_conv_factor)
# calculate the height of the RD4B using the final angle and based on
# the given equation.
height = 2 * self.ARM_LENGTH * (math.sin(final_angle_radians) -
self.INITIAL_ANGLE_RADIANS_SIN)
wpilib.SmartDashboard.putNumber("Lift Height", height)
wpilib.SmartDashboard.putNumber(
"Lift Angle", math.degrees(final_angle_radians)) # noqa: E501
wpilib.SmartDashboard.putNumber(
"Lift Position", self.lift_main.getSelectedSensorPosition(0))
wpilib.SmartDashboard.putNumber("Lift Error",
self.lift_main.getClosedLoopError(0))
def __set_lift_height(self, tgt_height):
final_angle_radians = math.asin((tgt_height / (2 * self.ARM_LENGTH)) +
self.INITIAL_ANGLE_RADIANS_SIN)
# convert this final angle from radians to native units.
native_units = ((final_angle_radians / angle_conv_factor) +
self.HORIZONTAL_ANGLE)
# set the left motor to run to this position (the right motor will
# follow it)
self.lift_main.set(TalonSRX.ControlMode.Position, native_units)
def robotInit(self):
self.stick = wpilib.Joystick(self.controller_index)
self.__prefs = wpilib.Preferences.getInstance()
self.lift_main = TalonSRX(self.main_lift_id)
self.lift_follower = TalonSRX(self.follower_id)
self.lift_main.configSelectedFeedbackSensor(
TalonSRX.FeedbackDevice.PulseWidthEncodedPosition, 0, 0)
self.lift_main.selectProfileSlot(0, 0)
self.lift_follower.set(TalonSRX.ControlMode.Follower,
self.main_lift_id)
self.last_out = 0
self.back = ButtonDebouncer(self.stick, 2)
self.fwd = ButtonDebouncer(self.stick, 3)
self.__load_config()
def disabledInit(self):
self.__load_config()
def disabledPeriodic(self):
self.__update_smart_dashboard()
def autonomousInit(self):
self.__load_config()
def autonomousPeriodic(self):
self.__set_lift_height(36)
self.__update_smart_dashboard()
def teleopInit(self):
self.__load_config()
def teleopPeriodic(self):
pct_pos = (self.stick.getRawAxis(self.control_axis_index) + 1) / 2
pct_pos = 1 - pct_pos
if self.stick.getRawButton(2):
pct_pos *= -1
elif not self.stick.getRawButton(3):
pct_pos = 0
self.last_out = pct_pos
pct_pos *= .25
self.lift_main.set(TalonSRX.ControlMode.PercentOutput, pct_pos)
self.__update_smart_dashboard()
class Claw():
claw_open_time = 0.5 # time to allow for the claw to open, in seconds
def __init__(self, talon_id, contact_sensor_channel):
"""
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.
Args:
talon_id: the ID number of the talon on the claw.
contact_sensor_channel: the channel that the contact sensor is
connected to.
"""
self.talon = TalonSRX(talon_id)
self.contact_sensor = wpilib.DigitalInput(contact_sensor_channel)
self.state = 'neutral'
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'
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.open_start_time = None
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.open_start_time = None
return
# if the state is neutral, clear the start time and set the talon speed
# to 0%
elif self.state == 'neutral':
self.open_start_time = None
self.talon.set(TalonSRX.ControlMode.PercentOutput, 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.PercentOutput, -1.0)
if self.contact_sensor.get(): # contact sensor pressed?
self.state = 'closed'
# if the state is "closed", use less power to the motors--
# Maintain grip, but don't squeeze too hard
elif self.state == 'closed':
self.talon.set(TalonSRX.ControlMode.PercentOutput, -0.05)
# 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_open_time, then transition to the "neutral"
# state.
elif self.state == 'opening':
cur_time = wpilib.Timer.getFPGATimestamp()
if self.open_start_time is None:
self.open_start_time = cur_time
self.talon.set(TalonSRX.ControlMode.PercentOutput, 1.0)
if cur_time - self.open_start_time > self.claw_open_time:
self.state = 'neutral'
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)