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