Package frc.spectrumLib.mechanism

Class Mechanism

java.lang.Object
frc.spectrumLib.mechanism.Mechanism
All Implemented Interfaces:
Subsystem
Direct Known Subclasses:
FuelIntake, Hood, IndexerBed, IndexerTower, IntakeExtension, IntakeExtension.IntakeExtensionRight, Launcher
public abstract class Mechanism extends Object implements Subsystem
Abstract base class representing a CTRE TalonFX-driven robot mechanism with common control modes, telemetry, and convenience Command factories.

This class centralizes:

  • Motor creation/configuration (leader + optional followers) via the provided Mechanism.Config
  • Cached sensor readings (position, velocity, voltage, current) for efficient access
  • Common unit conversions (rotations/percent/degrees; RPS/RPM)
  • Standard closed-loop and open-loop control helpers (Motion Magic, velocity, voltage, percent, torque current)
  • Convenience Trigger factories (at/above/below thresholds)
  • Basic periodic current reporting to a battery/current logger

Attachment semantics

If Mechanism.Config.isAttached() is false, the mechanism will not attempt to construct or command hardware and will return safe default sensor values (typically 0).

Target tracking

The target field tracks the last commanded closed-loop setpoint sent by this class. It is used by helper triggers such as atTargetPosition(...).

Extending

Concrete mechanisms should provide a Mechanism.Config describing motor IDs, Talon configuration, follower configuration, and mechanism-specific min/max rotation bounds as needed.

Note: This class assumes CTRE Phoenix 6 units (rotations, rotations/sec, etc.) and uses Config.talonConfig.Feedback.SensorToMechanismRatio as the mechanism gearing ratio.

  • Field Details

    • motor

      protected TalonFX motor
      The primary (leader) TalonFX motor controller.

    • followerMotors

      protected TalonFX[] followerMotors
      Optional follower TalonFX motor controllers that mirror the leader.

    • config

      public Mechanism.Config config
      Configuration object holding motor IDs, Talon settings, and mechanism parameters.

  • Constructor Details

    • Mechanism

      protected Mechanism(Mechanism.Config config)
      Creates a Mechanism and, if Mechanism.Config.isAttached() is true, initializes the leader TalonFX and any configured follower motors. Sensor caches are always initialized so safe defaults (0) are returned even when unattached.
      Parameters:
      config - the mechanism configuration (motor IDs, Talon settings, follower config, etc.)

    • Mechanism

      protected Mechanism(Mechanism.Config config, boolean attached)
      Creates a Mechanism and explicitly overrides the attached flag in the config.
      Parameters:
      config - the mechanism configuration
      attached - true to enable hardware; false to run in software-only mode

  • Method Details

    • periodic

      public void periodic()
      Called once per scheduler loop. Concrete subclasses should override to implement their periodic state-machine logic, telemetry, and sensor updates.
      Specified by:
      periodic in interface Subsystem

    • simulationPeriodic

      public void simulationPeriodic()
      Called once per simulation loop. Concrete subclasses should override to update simulation state (e.g., physics model inputs).
      Specified by:
      simulationPeriodic in interface Subsystem

    • getName

      public String getName()
      Returns the human-readable name of this mechanism, as defined in its Mechanism.Config.
      Specified by:
      getName in interface Subsystem
      Returns:
      the mechanism name

    • isAttached

      public boolean isAttached()
      Returns true if physical hardware is attached and motor commands should be sent.
      Returns:
      true when hardware is available

    • logBatteryUsage

      public void logBatteryUsage()
      Reports the combined supply current draw of the leader motor and all followers to the battery logger. Does nothing if the mechanism is not attached.

    • getCurrentCommandName

      protected String getCurrentCommandName()
      Returns the name of the command currently scheduled on this subsystem, or "none" if no command is running.
      Returns:
      the current command name

    • runningDefaultCommand

      public Trigger runningDefaultCommand()
      Returns a Trigger that is active whenever this subsystem's current command is its default command.
      Returns:
      trigger that is true while the default command is running

    • getTarget

      public double getTarget()
      Returns the last closed-loop setpoint (in rotations) sent to the motor by this class.
      Returns:
      the most recent target position in rotations

    • getVelocityTargetRPS

      public double getVelocityTargetRPS()
      Returns the last closed-loop velocity setpoint (in rotations per second) sent to the motor by this class.
      Returns:
      the most recent target velocity in rotations per second

    • atTargetPosition

      public Trigger atTargetPosition(DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor is within tolerance rotations of the last commanded target position.
      Parameters:
      tolerance - maximum allowable error in rotations
      Returns:
      trigger that is true when position error is within tolerance

    • isAtTargetPosition

      public boolean isAtTargetPosition(DoubleSupplier tolerance)
      Returns true when the motor is within tolerance rotations of the last commanded target position.
      Parameters:
      tolerance - maximum allowable error in rotations
      Returns:
      true when position error is within tolerance

    • atRotations

      public Trigger atRotations(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is within tolerance of target (both in rotations).
      Parameters:
      target - the desired position in rotations
      tolerance - maximum allowable error in rotations
      Returns:
      trigger that is true when position is within tolerance of target

    • isAtRotations

      public boolean isAtRotations(DoubleSupplier target, DoubleSupplier tolerance)
      Returns true when the motor position is within tolerance of target (both in rotations).
      Parameters:
      target - the desired position in rotations
      tolerance - maximum allowable error in rotations
      Returns:
      true when position is within tolerance of target

    • belowRotations

      public Trigger belowRotations(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is below target + tolerance rotations.
      Parameters:
      target - reference position in rotations
      tolerance - offset added to target to form the upper bound
      Returns:
      trigger that is true when position is below the threshold

    • aboveRotations

      public Trigger aboveRotations(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is above target - tolerance rotations.
      Parameters:
      target - reference position in rotations
      tolerance - offset subtracted from target to form the lower bound
      Returns:
      trigger that is true when position is above the threshold

    • atPercentage

      public Trigger atPercentage(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is within tolerance of target (both as a percentage of max rotations).
      Parameters:
      target - the desired position as a percentage of max rotations
      tolerance - maximum allowable error as a percentage
      Returns:
      trigger that is true when position is within tolerance of target

    • belowPercentage

      public Trigger belowPercentage(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is below target + tolerance (as a percentage of max rotations).
      Parameters:
      target - reference position as a percentage
      tolerance - offset added to target to form the upper bound
      Returns:
      trigger that is true when position is below the threshold

    • abovePercentage

      public Trigger abovePercentage(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is above target - tolerance (as a percentage of max rotations).
      Parameters:
      target - reference position as a percentage
      tolerance - offset subtracted from target to form the lower bound
      Returns:
      trigger that is true when position is above the threshold

    • atDegrees

      public Trigger atDegrees(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is within tolerance of target (both in degrees).
      Parameters:
      target - the desired position in degrees
      tolerance - maximum allowable error in degrees
      Returns:
      trigger that is true when position is within tolerance of target

    • belowDegrees

      public Trigger belowDegrees(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is below target + tolerance degrees.
      Parameters:
      target - reference position in degrees
      tolerance - offset added to target to form the upper bound
      Returns:
      trigger that is true when position is below the threshold

    • aboveDegrees

      public Trigger aboveDegrees(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor position is above target - tolerance degrees.
      Parameters:
      target - reference position in degrees
      tolerance - offset subtracted from target to form the lower bound
      Returns:
      trigger that is true when position is above the threshold

    • atVelocityRPM

      public Trigger atVelocityRPM(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor velocity is within tolerance RPM of target.
      Parameters:
      target - the desired velocity in RPM
      tolerance - maximum allowable error in RPM
      Returns:
      trigger that is true when velocity is within tolerance of target

    • belowVelocityRPM

      public Trigger belowVelocityRPM(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor velocity is below target + tolerance RPM.
      Parameters:
      target - reference velocity in RPM
      tolerance - offset added to target to form the upper bound
      Returns:
      trigger that is true when velocity is below the threshold

    • aboveVelocityRPM

      public Trigger aboveVelocityRPM(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor velocity is above target - tolerance RPM.
      Parameters:
      target - reference velocity in RPM
      tolerance - offset subtracted from target to form the lower bound
      Returns:
      trigger that is true when velocity is above the threshold

    • atCurrent

      public Trigger atCurrent(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor stator current is within tolerance amps of target.
      Parameters:
      target - the desired stator current in amps
      tolerance - maximum allowable error in amps
      Returns:
      trigger that is true when stator current is within tolerance of target

    • belowCurrent

      public Trigger belowCurrent(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor stator current is below target + tolerance amps.
      Parameters:
      target - reference current in amps
      tolerance - offset added to target to form the upper bound
      Returns:
      trigger that is true when stator current is below the threshold

    • aboveCurrent

      public Trigger aboveCurrent(DoubleSupplier target, DoubleSupplier tolerance)
      Returns a Trigger that is active when the motor stator current is above target - tolerance amps.
      Parameters:
      target - reference current in amps
      tolerance - offset subtracted from target to form the lower bound
      Returns:
      trigger that is true when stator current is above the threshold

    • updateStatorCurrent

      public double updateStatorCurrent()
      Reads the stator current directly from the motor hardware.
      Returns:
      motor stator current in amps, or 0 if not attached

    • getStatorCurrent

      public double getStatorCurrent()
      Returns the cached stator current of the motor.
      Returns:
      motor stator current in amps

    • updateSupplyCurrent

      public double updateSupplyCurrent()
      Reads the supply current directly from the motor hardware.
      Returns:
      motor supply current in amps, or 0 if not attached

    • getSupplyCurrent

      public double getSupplyCurrent()
      Returns the cached supply current of the motor.
      Returns:
      motor supply current in amps

    • updateVoltage

      public double updateVoltage()
      Reads the motor voltage directly from hardware.
      Returns:
      motor voltage in volts, or 0 if not attached

    • getVoltage

      public double getVoltage()
      Returns the cached voltage of the motor.
      Returns:
      motor voltage in volts

    • updateTemp

      public double updateTemp()
      Reads the motor temperature directly from hardware.
      Returns:
      motor temperature in Celsius, or 0 if not attached

    • getTemp

      public double getTemp()
      Returns the cached temperature of the motor.
      Returns:
      motor temperature in Celsius

    • percentToRotations

      public double percentToRotations(DoubleSupplier percent)
      Converts a percentage of the mechanism's maximum range into an absolute rotation count.
      Parameters:
      percent - position as a percentage of max rotations (0–100)
      Returns:
      the equivalent position in rotations

    • rotationsToPercent

      public double rotationsToPercent(DoubleSupplier rotations)
      Converts an absolute rotation count into a percentage of the mechanism's maximum range.
      Parameters:
      rotations - position in rotations
      Returns:
      the equivalent percentage of max rotations (0–100)

    • degreesToRotations

      public double degreesToRotations(DoubleSupplier degrees)
      Converts degrees to rotations (1 rotation = 360 degrees).
      Parameters:
      degrees - angle in degrees
      Returns:
      the equivalent position in rotations

    • rotationsToDegrees

      public double rotationsToDegrees(DoubleSupplier rotations)
      Converts rotations to degrees (1 rotation = 360 degrees).
      Parameters:
      rotations - position in rotations
      Returns:
      the equivalent angle in degrees

    • getPositionRotations

      public double getPositionRotations()
      Returns the cached motor position in rotations.
      Returns:
      motor position in rotations

    • getPositionPercentage

      public double getPositionPercentage()
      Returns the cached motor position as a percentage of max rotations.
      Returns:
      motor position in percentage of max rotations (0–100)

    • getPositionDegrees

      public double getPositionDegrees()
      Returns the cached motor position in degrees.
      Returns:
      motor position in degrees

    • getVelocityRPM

      public double getVelocityRPM()
      Returns the cached motor velocity in RPM.
      Returns:
      motor velocity in revolutions per minute

    • runVelocity

      public Command runVelocity(DoubleSupplier velocityRPM)
      Returns a Command that continuously drives the mechanism at the specified velocity using closed-loop voltage control.
      Parameters:
      velocityRPM - the target velocity in revolutions per minute
      Returns:
      a command that runs the mechanism at the given velocity

    • runVelocityTcFocRPM

      public Command runVelocityTcFocRPM(DoubleSupplier velocityRPM)
      Returns a Command that continuously drives the mechanism at the specified velocity using closed-loop Torque Current FOC control (requires Phoenix Pro).
      Parameters:
      velocityRPM - the target velocity in revolutions per minute
      Returns:
      a command that runs the mechanism at the given velocity using torque current FOC

    • runPercentage

      public Command runPercentage(DoubleSupplier percent)
      Returns a Command that continuously applies an open-loop percent output to the mechanism using voltage compensation.
      Parameters:
      percent - fractional output between -1 and +1
      Returns:
      a command that runs the mechanism at the given percent output

    • runVoltage

      public Command runVoltage(DoubleSupplier voltage)
      Returns a Command that continuously applies the specified voltage to the mechanism, bypassing any closed-loop control.
      Parameters:
      voltage - the desired voltage in volts
      Returns:
      a command that applies the given voltage output

    • runVoltageNoSoftLimit

      public Command runVoltageNoSoftLimit(DoubleSupplier voltage)
      Returns a Command that continuously applies the specified voltage to the mechanism, bypassing closed-loop control and ignoring software limit switches.
      Parameters:
      voltage - the desired voltage in volts
      Returns:
      a command that applies the given voltage output, ignoring software limits

    • runTorqueCurrentFoc

      public Command runTorqueCurrentFoc(DoubleSupplier current)
      Returns a Command that continuously drives the mechanism at the specified torque current using FOC control (requires Phoenix Pro).
      Parameters:
      current - the desired torque current in amps
      Returns:
      a command that runs the mechanism at the given torque current

    • moveToRotations

      public Command moveToRotations(DoubleSupplier rotations)
      Returns a Command that continuously moves the mechanism to the specified position using Motion Magic Torque Current FOC control (requires Phoenix Pro).
      Parameters:
      rotations - the target position in rotations
      Returns:
      a command that moves the mechanism to the given position

    • moveToPercentage

      public Command moveToPercentage(DoubleSupplier percent)
      Returns a Command that continuously moves the mechanism to the specified position using Motion Magic Torque Current FOC control (requires Phoenix Pro).
      Parameters:
      percent - the target position as a percentage of max rotations (0–100)
      Returns:
      a command that moves the mechanism to the given percentage position

    • moveToDegrees

      public Command moveToDegrees(DoubleSupplier degrees)
      Returns a Command that continuously moves the mechanism to the specified angular position using Motion Magic Torque Current FOC control (requires Phoenix Pro).
      Parameters:
      degrees - the target position in degrees
      Returns:
      a command that moves the mechanism to the given position in degrees

    • runFocRotations

      public Command runFocRotations(DoubleSupplier rotations)
      Returns a Command that continuously moves the mechanism to the specified position using Motion Magic Torque Current FOC control (requires Phoenix Pro).

      Equivalent to moveToRotations(DoubleSupplier) — prefer that method for clarity.

      Parameters:
      rotations - the target position in rotations
      Returns:
      a command that moves the mechanism to the given position

    • runStop

      public Command runStop()
      Returns a Command that stops the mechanism and holds it stopped for its duration.
      Returns:
      a command that stops the mechanism

    • coastMode

      public Command coastMode()
      Returns a Command that sets the mechanism to coast mode while active, then reverts to brake mode when the command ends. Safe to run while the robot is disabled.
      Returns:
      a command that temporarily enables coast mode

    • ensureBrakeMode

      public Command ensureBrakeMode()
      Returns a Command that sets the mechanism to brake mode if it is currently in coast mode. Safe to run while the robot is disabled.
      Returns:
      a command that ensures brake mode is active

    • runCurrentLimits

      protected Command runCurrentLimits(DoubleSupplier supplyLimit, DoubleSupplier statorLimit)
      Returns a Command that applies new supply and stator current limits to the mechanism.
      Parameters:
      supplyLimit - the new supply current limit in amps
      statorLimit - the new stator current limit in amps
      Returns:
      a command that updates the current limits

    • setCurrentLimits

      protected void setCurrentLimits(DoubleSupplier supplyLimit, DoubleSupplier statorLimit)
      Immediately applies new supply and stator current limits to the motor configuration.
      Parameters:
      supplyLimit - the new supply current limit in amps
      statorLimit - the new stator current limit in amps

    • stop

      protected void stop()
      Stops the motor output. Does nothing if the mechanism is not attached.

    • tareMotor

      protected void tareMotor()
      Sets the mechanism's reported position to zero (tares the motor encoder). Does nothing if the mechanism is not attached.

    • setMotorPosition

      protected void setMotorPosition(DoubleSupplier rotations)
      Sets the motor's internal position register to the specified value without moving the motor.
      Parameters:
      rotations - the position to write to the motor in rotations

    • setMMVelocityFOC

      protected void setMMVelocityFOC(DoubleSupplier velocityRPS)
      Closed-loop velocity control using Motion Magic with Torque Current FOC (requires Phoenix Pro).
      Parameters:
      velocityRPS - the target velocity in rotations per second

    • setVelocityTorqueCurrentFOC

      protected void setVelocityTorqueCurrentFOC(DoubleSupplier velocityRPS)
      Closed-loop velocity control using Torque Current FOC (requires Phoenix Pro).
      Parameters:
      velocityRPS - the target velocity in rotations per second

    • setVelocityTCFOCrpm

      protected void setVelocityTCFOCrpm(DoubleSupplier velocityRPM)
      Closed-loop velocity control using Torque Current FOC with an RPM input (requires Phoenix Pro). The RPM value is converted to RPS internally before being sent to the motor.
      Parameters:
      velocityRPM - the target velocity in revolutions per minute

    • setVelocity

      protected void setVelocity(DoubleSupplier velocityRPS)
      Closed-loop velocity control with voltage compensation.
      Parameters:
      velocityRPS - the target velocity in rotations per second

    • setMMPositionFoc

      protected void setMMPositionFoc(DoubleSupplier rotations)
      Closed-loop position control using Motion Magic with Torque Current FOC (requires Phoenix Pro).
      Parameters:
      rotations - the target position in rotations

    • setDynMMPositionFoc

      protected void setDynMMPositionFoc(DoubleSupplier rotations, DoubleSupplier velocity, DoubleSupplier acceleration, DoubleSupplier jerk)
      Closed-loop position control using Dynamic Motion Magic with Torque Current FOC (requires Phoenix Pro). Trajectory parameters can be changed every loop cycle.
      Parameters:
      rotations - the target position in rotations
      velocity - the cruise velocity in rotations per second
      acceleration - the acceleration in rotations per second squared
      jerk - the jerk in rotations per second cubed

    • setDynMMPositionVoltage

      protected void setDynMMPositionVoltage(DoubleSupplier rotations, DoubleSupplier velocity, DoubleSupplier acceleration, DoubleSupplier jerk)
      Closed-loop position control using Dynamic Motion Magic with voltage compensation. Trajectory parameters can be changed every loop cycle.
      Parameters:
      rotations - the target position in rotations
      velocity - the cruise velocity in rotations per second
      acceleration - the acceleration in rotations per second squared
      jerk - the jerk in rotations per second cubed

    • setMMPosition

      protected void setMMPosition(DoubleSupplier rotations)
      Closed-loop position control using Motion Magic with voltage compensation (slot 0).
      Parameters:
      rotations - the target position in rotations

    • setMMPosition

      protected void setMMPosition(DoubleSupplier rotations, int slot)
      Closed-loop position control using Motion Magic with voltage compensation and an explicit PID/FF gain slot.
      Parameters:
      rotations - the target position in rotations
      slot - the gain slot to use (0, 1, or 2)

    • setPercentOutput

      protected void setPercentOutput(DoubleSupplier percent)
      Open-loop percent output control with voltage compensation. The output voltage is percent × voltageCompSaturation.
      Parameters:
      percent - fractional output between -1 and +1

    • setVoltageOutput

      protected void setVoltageOutput(DoubleSupplier voltage)
      Open-loop voltage control — applies the requested voltage directly without compensation scaling.
      Parameters:
      voltage - the desired voltage in volts

    • setVoltageOutputNoSoftLimit

      protected void setVoltageOutputNoSoftLimit(DoubleSupplier voltage)
      Open-loop voltage control that ignores software limit switches. Use with caution — this can drive the mechanism past its configured travel limits.
      Parameters:
      voltage - the desired voltage in volts

    • setTorqueCurrentFoc

      public void setTorqueCurrentFoc(DoubleSupplier current)
      Applies a torque current setpoint using FOC control (requires Phoenix Pro).
      Parameters:
      current - the desired torque current in amps

    • setBrakeMode

      public void setBrakeMode(boolean isInBrake)
      Sets the motor's neutral mode to brake or coast and immediately applies the change to hardware.
      Parameters:
      isInBrake - true to set brake mode; false to set coast mode

    • toggleReverseSoftLimit

      public void toggleReverseSoftLimit(boolean enabled)
      Enables or disables the reverse software limit switch and immediately applies the change. The threshold is read from the current configuration.
      Parameters:
      enabled - true to enable the reverse soft limit; false to disable it

    • toggleTorqueCurrentLimit

      public void toggleTorqueCurrentLimit(DoubleSupplier enabledLimit, boolean enabled)
      Enables or disables a forward/reverse torque current limit and immediately applies the change. When disabled, the peak torque current is reset to ±300 A (effectively unlimited).
      Parameters:
      enabledLimit - the torque current limit in amps when enabled is true
      enabled - true to apply the limit; false to remove it

    • toggleSupplyCurrentLimit

      public void toggleSupplyCurrentLimit(DoubleSupplier enabledLimit, boolean enabled)
      Enables or disables the supply current limit and immediately applies the change.
      Parameters:
      enabledLimit - the supply current limit in amps
      enabled - true to enable the limit; false to disable it

    • applyCurrentLimit

      public void applyCurrentLimit(DoubleSupplier supplyLimit, DoubleSupplier statorLimit)
      Applies new supply and stator current limits if the requested values differ from the currently configured limits. The update is retried up to 10 times on failure.
      Parameters:
      supplyLimit - the new supply current limit in amps
      statorLimit - the new stator current limit in amps

    • checkAvgCurrent

      public Command checkAvgCurrent(DoubleSupplier expectedCurrent, DoubleSupplier tolerance)
      Returns a Command that measures the average stator current over its runtime and fires a warning Alert if the average deviates from expectedCurrent by more than tolerance.
      Parameters:
      expectedCurrent - the expected average stator current in amps
      tolerance - the maximum acceptable deviation in amps
      Returns:
      a diagnostic command that checks average current

    • checkMaxCurrent

      public Command checkMaxCurrent(DoubleSupplier expectedCurrent)
      Returns a Command that tracks the peak stator current over its runtime and fires a warning Alert if the peak exceeds expectedCurrent.
      Parameters:
      expectedCurrent - the maximum acceptable peak stator current in amps
      Returns:
      a diagnostic command that checks peak current

    • checkMinThresholdCurrent

      public Command checkMinThresholdCurrent(DoubleSupplier expectedCurrent)
      Returns a Command that tracks the peak stator current over its runtime and fires a warning Alert if the peak never reaches expectedCurrent. Use this to verify that a mechanism drew at least the expected minimum load.
      Parameters:
      expectedCurrent - the minimum acceptable peak stator current in amps
      Returns:
      a diagnostic command that checks whether a minimum current threshold was reached

    • getMotor

      public TalonFX getMotor()
      The primary (leader) TalonFX motor controller.

    • getFollowerMotors

      public TalonFX[] getFollowerMotors()
      Optional follower TalonFX motor controllers that mirror the leader.