Package frc.robot.subsystems.turret.commands

Class TrackFieldTargetCommand

java.lang.Object

edu.wpi.first.wpilibj2.command.Command

frc.robot.shared.commands.AbstractSubsystemCommand<TurretSubsystem>

frc.robot.subsystems.turret.commands.TrackFieldTargetCommand

All Implemented Interfaces:

edu.wpi.first.util.sendable.Sendable

public class TrackFieldTargetCommand
extends AbstractSubsystemCommand<TurretSubsystem>

Continuously aims the turret at a field-relative target position with shoot-on-the-move compensation.

This command reads the fused robot pose each loop, runs the SOTM solver to compute where the turret should aim (accounting for robot velocity and ball flight time), and drives the turret using the set-and-seek profile. It does not finish on its own and should be interrupted when tracking is no longer required.

Nested Class Summary

Nested classes/interfaces inherited from class edu.wpi.first.wpilibj2.command.Command

edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior

Field Summary

Fields inherited from class frc.robot.shared.commands.AbstractSubsystemCommand

log, subsystem

Constructor Summary

Constructors

Constructor	Description
TrackFieldTargetCommand(TurretSubsystem turretSubsystem, RobotPoseSubsystem robotPoseSubsystem, Supplier<edu.wpi.first.math.geometry.Translation2d> targetFieldPositionSupplier, Supplier<Double> robotYawRateRadiansPerSecondSupplier, Supplier<edu.wpi.first.math.kinematics.ChassisSpeeds> fieldVelocitySupplier, DoubleUnaryOperator tofLookup, ShootOnTheMoveCalculator sotmCalculator)	Creates a command that tracks a field-relative target position with shoot-on-the-move compensation.

Method Summary

Modifier and Type	Method	Description
void	end(boolean interrupted)	Notifies the turret that tracking was interrupted so it can reset its profile state.
void	execute()	Recomputes the turret target and drives the profile toward it each cycle.
double	getCompensatedDistanceMeters()	Returns the most recent compensated distance computed by the SOTM solver.
boolean	isFinished()	Returns false so the command tracks continuously until interrupted.
protected void	onInitialize()	Seeds the turret target with the current field-relative angle on first run.

Methods inherited from class frc.robot.shared.commands.AbstractSubsystemCommand

initialize

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

addRequirements, addRequirements, alongWith, andThen, andThen, asProxy, beforeStarting, beforeStarting, cancel, deadlineFor, deadlineWith, finallyDo, finallyDo, getInterruptionBehavior, getName, getRequirements, getSubsystem, handleInterrupt, hasRequirement, ignoringDisable, initSendable, isScheduled, onlyIf, onlyWhile, raceWith, repeatedly, runsWhenDisabled, schedule, setName, setSubsystem, unless, until, withDeadline, withInterruptBehavior, withName, withTimeout, withTimeout

Methods inherited from class java.lang.Object

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

Constructor Details

TrackFieldTargetCommand

public TrackFieldTargetCommand(TurretSubsystem turretSubsystem,
 RobotPoseSubsystem robotPoseSubsystem,
 Supplier<edu.wpi.first.math.geometry.Translation2d> targetFieldPositionSupplier,
 Supplier<Double> robotYawRateRadiansPerSecondSupplier,
 Supplier<edu.wpi.first.math.kinematics.ChassisSpeeds> fieldVelocitySupplier,
 DoubleUnaryOperator tofLookup,
 ShootOnTheMoveCalculator sotmCalculator)

Creates a command that tracks a field-relative target position with shoot-on-the-move compensation.

The target supplier should return the current target location in meters on the field coordinate system. The yaw rate supplier provides the robot's current rotational velocity so the turret can lead its aim while the robot spins. The field velocity supplier and TOF lookup feed the SOTM solver so the turret compensates for ball drift during translational motion.

Parameters:

turretSubsystem - turret subsystem to control

robotPoseSubsystem - robot pose subsystem providing the fused pose estimate

targetFieldPositionSupplier - supplier of the field-relative target position in meters

robotYawRateRadiansPerSecondSupplier - supplier of the robot's yaw rate in radians per second (positive is counter-clockwise)

fieldVelocitySupplier - supplier of the robot's field-relative velocity for SOTM compensation

tofLookup - function returning estimated time of flight given a distance in meters

sotmCalculator - shoot-on-the-move solver instance

Method Details

getCompensatedDistanceMeters

public double getCompensatedDistanceMeters()

Returns the most recent compensated distance computed by the SOTM solver.

Use this as the distance supplier for the shooter RPM lookup during fire-ready so the flywheel speed matches the effective shot distance rather than the raw straight-line distance.

Returns:

compensated distance in meters from the launcher to the virtual target

execute

public void execute()

Recomputes the turret target and drives the profile toward it each cycle.

Overrides:

execute in class edu.wpi.first.wpilibj2.command.Command

end

public void end(boolean interrupted)

Notifies the turret that tracking was interrupted so it can reset its profile state.

Overrides:

end in class edu.wpi.first.wpilibj2.command.Command

Parameters:

interrupted - true when the command was interrupted rather than finishing normally

isFinished

public boolean isFinished()

Returns false so the command tracks continuously until interrupted.

Overrides:

isFinished in class edu.wpi.first.wpilibj2.command.Command

Returns:

always false

onInitialize

protected void onInitialize()

Seeds the turret target with the current field-relative angle on first run.

Overrides:

onInitialize in class AbstractSubsystemCommand<TurretSubsystem>