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