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