Package frc.robot.subsystems.turret

Class TurretSubsystem

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

frc.robot.subsystems.turret.TurretSubsystem

All Implemented Interfaces:

edu.wpi.first.util.sendable.Sendable, edu.wpi.first.wpilibj2.command.Subsystem

public class TurretSubsystem
extends AbstractSetAndSeekSubsystem<TurretSubsystemConfig>

Turret subsystem with a single profiled motor. Exposes the set-and-seek API so callers can drive to angles in degrees while the superclass handles motion profiling, limits, and logging.

Field Summary

Fields inherited from class frc.robot.shared.subsystems.AbstractSetAndSeekSubsystem

constraints, controller, goalState, setpointState, targetWasClamped

Fields inherited from class frc.robot.shared.subsystems.AbstractMotorSubsystem

feedforward, motor, motorInputs

Fields inherited from class frc.robot.shared.subsystems.AbstractSubsystem

className, config, enabled, kDt, log, verbose

Constructor Summary

Constructors

Constructor	Description
TurretSubsystem(TurretSubsystemConfig config)	Builds the turret subsystem with a single SparkMax-driven motor and default motion profile values.

Method Summary

Modifier and Type	Method	Description
double	calculateFieldTargetDegrees(edu.wpi.first.math.geometry.Pose2d robotPose, edu.wpi.first.math.geometry.Translation2d targetFieldPositionMeters, double robotYawRateRadiansPerSecond)	Computes the turret target angle needed to face a field-relative target while compensating for robot rotation.
edu.wpi.first.math.geometry.Translation2d	getTurretFieldPosition(edu.wpi.first.math.geometry.Pose2d robotPose)	Computes the turret pivot position on the field by transforming the configured component offset from robot-relative to field-relative coordinates.
boolean	isOnTarget()	Checks whether the turret is aimed close enough to shoot, ignoring velocity.
void	setTarget(double targetPositionDegrees)	Sets a new turret goal after normalizing the angle into the [-180, 180] range.
void	simulationPeriodic()	Publishes the simulated turret's 3D pose for AdvantageScope animation.

Methods inherited from class frc.robot.shared.subsystems.AbstractSetAndSeekSubsystem

applySetpoint, calculateFeedforward, getMeasuredPosition, getMeasuredPositionDegrees, getMeasuredVelocity, handleSeekInterrupted, isProfileSettled, periodic, resetEncoderPosition, seekTarget, settleAtCurrentPosition, stop

Methods inherited from class frc.robot.shared.subsystems.AbstractMotorSubsystem

applyVoltage, getMeasuredPositionRadians, getMeasuredVelocityRadiansPerSecond, getSysIdRoutine, logSysIdEnd, logSysIdInterrupted, logSysIdStart, refreshFeedforward, updateAndLogMotorInputs

Methods inherited from class frc.robot.shared.subsystems.AbstractSubsystem

getConfig, isFMSAttached, isSimulation, isSubsystemDisabled, isVerbose, isVerboseLoggingEnabled, logDisabled, reportWarning

Methods inherited from class edu.wpi.first.wpilibj2.command.SubsystemBase

addChild, getName, getSubsystem, initSendable, setName, setSubsystem

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Methods inherited from interface edu.wpi.first.wpilibj2.command.Subsystem

defer, getCurrentCommand, getDefaultCommand, idle, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, startEnd, startRun

Constructor Details

TurretSubsystem

public TurretSubsystem(TurretSubsystemConfig config)

Builds the turret subsystem with a single SparkMax-driven motor and default motion profile values.

Parameters:

config - turret configuration bundle loaded from JSON; angles are expressed in degrees

Method Details

simulationPeriodic

public void simulationPeriodic()

Publishes the simulated turret's 3D pose for AdvantageScope animation.

The pose combines the configured pivot offset with a Z-axis (yaw) rotation matching the measured turret angle, adjusted by the turret zero offset so 0° points backward in simulation. This ensures the simulated model matches the real robot's orientation when the turret is rear-facing.

getTurretFieldPosition

public edu.wpi.first.math.geometry.Translation2d getTurretFieldPosition(edu.wpi.first.math.geometry.Pose2d robotPose)

Computes the turret pivot position on the field by transforming the configured component offset from robot-relative to field-relative coordinates.

The offset comes from componentPoseConfig (the same values used for AdvantageScope rendering), so the sim model and the aim calculations always agree on where the turret sits. This method is used by calculateFieldTargetDegrees(edu.wpi.first.math.geometry.Pose2d, edu.wpi.first.math.geometry.Translation2d, double) and by the shoot-on-the-move solver for distance and velocity calculations.

Parameters:

robotPose - current robot pose in meters and radians

Returns:

turret pivot position on the field in meters

calculateFieldTargetDegrees

public double calculateFieldTargetDegrees(edu.wpi.first.math.geometry.Pose2d robotPose,
 edu.wpi.first.math.geometry.Translation2d targetFieldPositionMeters,
 double robotYawRateRadiansPerSecond)

Computes the turret target angle needed to face a field-relative target while compensating for robot rotation.

The returned angle is relative to the turret's own zero direction, which is defined by turretZeroOffsetDegrees. For a rear-facing turret (offset = 180°), turret 0° points straight backward, positive angles rotate counter-clockwise when viewed from above, and the configured setpoint limits bound how far the turret can swing from its zero. The formula is:

turretAngle = fieldAngleToTarget − robotHeading − turretZeroOffset

The angle is measured from the turret's physical pivot point (defined by componentPoseConfig) rather than from the robot center. This eliminates a small aim error that would otherwise grow at close range.

Rotational lead-time compensation predicts where the robot heading will be after a short look-ahead period and subtracts that predicted change so the turret pre-rotates instead of lagging behind. The result is clamped to the configured turret limits.

Parameters:

robotPose - current robot pose in meters and radians

targetFieldPositionMeters - target position on the field in meters

robotYawRateRadiansPerSecond - current robot rotational velocity in radians per second (positive is counter-clockwise)

Returns:

turret target angle in degrees, relative to turret zero

setTarget

public void setTarget(double targetPositionDegrees)

Sets a new turret goal after normalizing the angle into the [-180, 180] range.

The superclass clamp then detects whether the normalized angle falls outside the turret's setpoint limits and flags it so isOnTarget() correctly returns false for unreachable targets.

Overrides:

setTarget in class AbstractSetAndSeekSubsystem<TurretSubsystemConfig>

Parameters:

targetPositionDegrees - desired turret position in degrees, relative to turret zero

isOnTarget

public boolean isOnTarget()

Checks whether the turret is aimed close enough to shoot, ignoring velocity.

Returns false when the requested target was clamped (target is outside the turret's reachable arc). Otherwise compares the position error against onTargetPositionToleranceDegrees from the turret config.

Returns:

true when the turret is within the on-target tolerance of its goal