Package frc.robot.shared.subsystems

Class AbstractSetAndSeekSubsystem<TConfig extends AbstractSetAndSeekSubsystemConfig>

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
frc.robot.shared.subsystems.AbstractSubsystem<TConfig>
frc.robot.shared.subsystems.AbstractMotorSubsystem<TConfig>
frc.robot.shared.subsystems.AbstractSetAndSeekSubsystem<TConfig>
All Implemented Interfaces:
edu.wpi.first.util.sendable.Sendable, edu.wpi.first.wpilibj2.command.Subsystem
Direct Known Subclasses:
HarvesterSubsystem, TurretSubsystem
public abstract class AbstractSetAndSeekSubsystem<TConfig extends AbstractSetAndSeekSubsystemConfig> extends AbstractMotorSubsystem<TConfig>
Base subsystem that generates and follows a trapezoidal motion profile.

Concrete mechanisms should extend this class to gain a simple "set and seek" API: call setTarget(double) to define a goal and repeatedly call seekTarget() from a command to step the profile forward. Motor control, feedforward, SysId support, and AdvantageKit input logging are provided by the AbstractMotorSubsystem parent class.

  • Field Details

    • constraints

      protected edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints constraints
      Trapezoid constraints that bound velocity and acceleration.

    • controller

      protected final edu.wpi.first.math.controller.ProfiledPIDController controller
      Profiled PID controller that tracks the trapezoid setpoint.

    • goalState

      protected edu.wpi.first.math.trajectory.TrapezoidProfile.State goalState
      Desired goal state for the trapezoid profile.

    • setpointState

      protected edu.wpi.first.math.trajectory.TrapezoidProfile.State setpointState
      Current setpoint state produced by the trapezoid profile.

    • targetWasClamped

      protected boolean targetWasClamped
      True when the most recent target was clamped to the setpoint limits, meaning the mechanism cannot reach the requested position.

  • Constructor Details

    • AbstractSetAndSeekSubsystem

      protected AbstractSetAndSeekSubsystem(TConfig config, Motor motor)
      Creates a profiled subsystem with bounded setpoints, motion constraints, and a single motor.
      Parameters:
      config - Configuration values that define the allowable range, motion limits, and initial state.
      motor - Motor controller that reports position/velocity and accepts duty-cycle commands, or null to use a disabled no-op motor.

  • Method Details

    • periodic

      public void periodic()
      Refreshes motor configuration and profile constraints when not attached to the FMS.

      Override this if you need additional periodic behavior, but call super.periodic() to keep live tuning updates active.

      Specified by:
      periodic in interface edu.wpi.first.wpilibj2.command.Subsystem
      Overrides:
      periodic in class AbstractMotorSubsystem<TConfig extends AbstractSetAndSeekSubsystemConfig>

    • setTarget

      public void setTarget(double targetPositionDegrees)
      Sets a new goal for the motion profile. Values outside the configured range are clamped.
      Parameters:
      targetPositionDegrees - desired mechanism position in degrees

    • seekTarget

      public void seekTarget()
      Advances the trapezoidal profile by one cycle and hands the setpoint to the subclass for actuation.

    • stop

      public void stop()
      Resets the profiled controller and goal state, then stops the motor.

      Use this for a full stop that clears all profile state. For a light-touch interruption that preserves profile state (so settle commands can decelerate smoothly), use handleSeekInterrupted() instead.

      Overrides:
      stop in class AbstractMotorSubsystem<TConfig extends AbstractSetAndSeekSubsystemConfig>

    • resetEncoderPosition

      public void resetEncoderPosition()
      Resets the motor encoder to the configured initial position and reinitializes the profile state.

      Call this from a dashboard button or utility command after manually repositioning a mechanism back to its starting position (e.g., stowing a harvester arm or centering a turret by hand). The motor is stopped before the reset to avoid applying stale voltage at the new position.

    • handleSeekInterrupted

      public void handleSeekInterrupted()
      Hook for subclasses to respond when a seek command is interrupted. Default implementation stops the motor without resetting profile state.

      Settle commands depend on the preserved profile state to decelerate smoothly. If you need a full reset, call stop() instead.

    • settleAtCurrentPosition

      public void settleAtCurrentPosition()
      Retargets the profile using the latest measured position and velocity to settle without oscillation.

      Call this when interrupting a moving command so the controller decelerates smoothly from the current motion state.

    • isProfileSettled

      public boolean isProfileSettled()
      States whether the profiled controller is within both position and velocity tolerances of its goal.
      Returns:
      True when the profile reports settled.

    • getMeasuredPositionDegrees

      public double getMeasuredPositionDegrees()
      Returns the current measured position in degrees.

      Delegates to getMeasuredPosition() and converts from radians. Use this for external consumers such as suppliers and telemetry.

      Returns:
      measured mechanism position in degrees

    • calculateFeedforward

      protected double calculateFeedforward(double previousSetpointVelocity, edu.wpi.first.math.trajectory.TrapezoidProfile.State currentSetpoint)
      Computes the feedforward voltage for the current profile step.

      The default implementation uses the parent's SimpleMotorFeedforward with discrete-time plant inversion (the two-velocity form, so kA contributes the acceleration torque the profile demands). Subclasses that need a different feedforward model (e.g., ArmFeedforward for gravity compensation) should override this method.

      Parameters:
      previousSetpointVelocity - velocity setpoint from the previous cycle in radians per second
      currentSetpoint - the setpoint state the profile wants us to follow this cycle
      Returns:
      feedforward voltage in volts

    • applySetpoint

      protected void applySetpoint(edu.wpi.first.math.trajectory.TrapezoidProfile.State setpoint, double voltageCommand)
      Applies the calculated setpoint to hardware. Override to customize control behavior.
      Parameters:
      setpoint - the next state from the trapezoidal profile
      voltageCommand - requested motor voltage in volts before clamping

    • getMeasuredPosition

      protected double getMeasuredPosition()
      Provides the measured mechanism position. Override to read from an encoder or other sensor.
      Returns:
      The current measured position in radians. Defaults to the motor's reported position.

    • getMeasuredVelocity

      protected double getMeasuredVelocity()
      Provides the measured mechanism velocity. Override to read from an encoder or other sensor.
      Returns:
      The current measured velocity in radians per second. Defaults to the motor's reported velocity.