Package frc.robot.shared.targeting

Class ShootOnTheMoveCalculator

java.lang.Object
frc.robot.shared.targeting.ShootOnTheMoveCalculator
public class ShootOnTheMoveCalculator extends Object
Shoot-on-the-move solver that computes a compensated aim point and distance for the turret.

When the robot is moving, a ball launched from the turret inherits the robot's velocity. During its time of flight the ball drifts in the direction of travel, causing it to overshoot the target if the turret aims straight at it. This solver computes the "virtual" target the turret should aim at so the ball arrives at the real target after accounting for that drift.

The math is adapted from the frc-fire-control ShotCalculator (MIT, FRC 5962). The key difference is that this solver returns a compensated Translation2d aim point for the turret instead of a whole-robot drive heading, because our robot has an independent turret.

  • Nested Class Summary

    Nested Classes
    Modifier and Type
    Class
    Description
    static final record 
    ShootOnTheMoveCalculator.ShotSolution
    Immutable result of the SOTM solve.

  • Constructor Summary

    Constructors
    Constructor
    Description
    ShootOnTheMoveCalculator(double dragCoefficient, double minSpeedMetersPerSecond, int maxIterations, double convergenceToleranceSeconds)
    Creates a solver with the given tuning parameters.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    resetWarmStart()
    Resets the warm-start state.
    ShootOnTheMoveCalculator.ShotSolution
    solve(edu.wpi.first.math.geometry.Translation2d launcherFieldPosition, edu.wpi.first.math.kinematics.ChassisSpeeds fieldVelocity, double turretPivotX, double turretPivotY, double robotHeadingRadians, edu.wpi.first.math.geometry.Translation2d targetPosition, DoubleUnaryOperator tofLookup)
    Solves for the compensated aim point given the robot's current state.

    Methods inherited from class java.lang.Object

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

  • Constructor Details

    • ShootOnTheMoveCalculator

      public ShootOnTheMoveCalculator(double dragCoefficient, double minSpeedMetersPerSecond, int maxIterations, double convergenceToleranceSeconds)
      Creates a solver with the given tuning parameters.
      Parameters:
      dragCoefficient - aerodynamic drag coefficient for the ball's inherited velocity decay during flight (0.47 for a smooth sphere)
      minSpeedMetersPerSecond - robot speed below which SOTM is disabled and the turret aims straight at the target
      maxIterations - maximum Newton iterations before falling back to the initial guess
      convergenceToleranceSeconds - convergence threshold for the Newton solver in seconds

  • Method Details

    • solve

      public ShootOnTheMoveCalculator.ShotSolution solve(edu.wpi.first.math.geometry.Translation2d launcherFieldPosition, edu.wpi.first.math.kinematics.ChassisSpeeds fieldVelocity, double turretPivotX, double turretPivotY, double robotHeadingRadians, edu.wpi.first.math.geometry.Translation2d targetPosition, DoubleUnaryOperator tofLookup)
      Solves for the compensated aim point given the robot's current state.

      The solver accounts for the turret's physical offset from robot center by computing the launcher velocity as the robot center velocity plus the rotational contribution from the turret offset. This matters because the turret traces an arc when the robot rotates.

      Parameters:
      launcherFieldPosition - field-relative position of the turret pivot in meters (from TurretSubsystem.getTurretFieldPosition(edu.wpi.first.math.geometry.Pose2d))
      fieldVelocity - robot center velocity in the field frame (vx, vy, omega)
      turretPivotX - turret pivot X offset from robot center in meters (positive = forward)
      turretPivotY - turret pivot Y offset from robot center in meters (positive = left)
      robotHeadingRadians - current robot heading in radians
      targetPosition - field-relative target position in meters
      tofLookup - function that returns estimated time of flight given a distance in meters
      Returns:
      solved shot solution with compensated aim point and distance

    • resetWarmStart

      public void resetWarmStart()
      Resets the warm-start state. Call after a pose reset so the solver does not use stale data.