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