Turret improvements

[therekrab]

This adds support for a turret in three dimensions, as well as a constrained SOTM algorithm that limits pitch and max velocity to make sure we can make the shot. I also implemented ToF recursion for shooting on the move.

[dkbrown8]

PR #57 Review - Turret Improvements

Overall Assessment

This is a substantial and well-structured refactor. The key improvements — adding AimConstraints, fixing alliance flipping, switching to turret pose offset, and adding AimStatus — are all solid design decisions. Good test coverage added too. However, there are a few bugs worth addressing.

---

Bugs

1. Mutable shared kImpossible instance (AimParams.java:97)

public static final AimParams kImpossible = new AimParams().withStatus(AimStatus.Impossible);

public AimParams withStatus(AimStatus status) {
    this.status = status; // mutates this!
    return this;
}

withStatus mutates and returns this. Since StateManager.params can be assigned kImpossible directly, any caller doing params.withStatus(...) would corrupt the shared constant. withStatus should return a new AimParams copy, not mutate in place.

2. Binary search dead loop in PhysicsAim.update() (PhysicsAim.java:303-321)
If ok=false due to velocity exceeding maxVelocity (not a pitch constraint violation), neither pitch branch updates lower/upper. Every iteration will compute the same guess and reach the same result, wasting all iterations without converging. The velocity case should adjust the search bounds.

3. Ignored params parameter in FuelShotSim.launch() (FuelShotSim.java:447)

public void launch(StateManager state, AimParams params) {
    params = state.aimParams(); // overwrites the parameter immediately

The passed-in params is always discarded. Either remove the parameter from the method signature, or use it as intended.

---

Issues

4. Typo: kAllianzeZoneLength (Constants.java:21)
Both the field name and comment say "Allianze" — should be "Alliance" / kAllianceZoneLength. Also referenced in FieldUtils.java:717.

5. kFeedTarget has no unit comments (Constants.java:24)

public static final Pose3d kFeedTarget = new Pose3d(4.5, 2, 1.0, Rotation3d.kZero);

All other poses/dimensions have javadoc. A comment explaining what z=1.0 represents (e.g. height of the feed target) would be consistent.

---

Positives

• Renaming ExperimentalAim → EmpiricalAim is more accurate
• Converting AimStrategy from abstract class to interface is cleaner
• Alliance flip logic in FieldUtils was broken (returning null for Red) — now fixed correctly
• SingleInputPoseEstimator xMax/yMax swap fix is correct (X = length, Y = width)
• Centralizing AimParams logging in StateManager instead of in AimParams itself is better separation of concerns
• PhysicsAim.quicksolve being public static makes it easily testable — good decision
• New tests for PhysicsAim and Turret are well-written

[dkbrown8]

Suggested Fixes for Reported Bugs

---

Fix 1: Mutable shared kImpossible — AimParams.java

withStatus should copy the object instead of mutating it:

public AimParams withStatus(AimStatus status) {
    AimParams copy = new AimParams();
    copy.pitch = this.pitch;
    copy.yaw = this.yaw;
    copy.velocity = this.velocity;
    copy.deltaPitch = this.deltaPitch;
    copy.deltaYaw = this.deltaYaw;
    copy.deltaVelocity = this.deltaVelocity;
    copy.status = status;
    return copy;
}

This ensures kImpossible is never mutated by callers.

---

Fix 2: Binary search stall when velocity exceeds max — PhysicsAim.java

Add a velocity-over-max case inside the loop to update the bounds when pitch is valid but velocity is too high:

for (int i = 0; i < ITERATIONS; i++) {
    double guess = 0.5 * (lower + upper);
    AimParams output = quicksolve(offset, robotVelocity, guess);
    boolean ok = constraints.check(output);
    if (ok) {
        upper = guess;
        best = output.withStatus(AimStatus.Possible);
    }
    double pitch = output.pitch.getRadians();
    if (pitch > constraints.maxShooterAngle().getRadians()) {
        upper = guess;
    } else if (pitch < constraints.minShooterAngle().getRadians()) {
        lower = guess;
    } else if (!ok) {
        // Pitch is valid but velocity is too high — reduce descent speed
        upper = guess;
    }
}

---

Fix 3: Ignored params parameter — FuelShotSim.java

Remove the unused parameter from the method signature:

// Before
public void launch(StateManager state, AimParams params) {
    params = state.aimParams();
    ...
}

// After
public void launch(StateManager state) {
    AimParams params = state.aimParams();
    ...
}

Update the call site to match the new signature.

---

Fix 4 (Issue): Typo kAllianzeZoneLength — Constants.java + FieldUtils.java

Rename to kAllianceZoneLength and fix the comment spelling in both files.

[dkbrown8]

Updated Review (based on latest commits)

Several findings from the previous reviews have been addressed: the kImpossible mutation bug is fixed via constructor, the ignored params parameter in FuelShotSim is fixed, the drag simulation is restored, the kAllianzeZoneLength typo is fixed, and EmpiricalAim/PolyRegAim are gone so those concerns are moot.

---

Bug 1: ToFAim.ITERATIONS declared as double (ToFAim.java)

static final double ITERATIONS = 5;

Used as a loop bound (i < ITERATIONS). Should be int, matching PhysicsAim.ITERATIONS. While Java will coerce this, it's semantically wrong — PhysicsAim already uses private static final int ITERATIONS = 5 as the correct pattern.

---

Bug 2: ToFAim.constraints.check() with MechanismControl is semantically invalid (ToFAim.java)

params.status = (constraints.check(params)) ? AimStatus.Possible : AimStatus.Impossible;

AimConstraints.check() compares params.velocity <= maxVelocity(). But ToFAim sets params.velocity = shooterControl under SpeedControl.MechanismControl — a mechanism control value, not a projectile velocity in m/s. Comparing it against maxVelocity = 18 (m/s) is meaningless. The velocity portion of the constraint check should be skipped when SpeedControl.MechanismControl is used.

---

Bug 3: withSpeedControl() still mutates in place (AimParams.java)

The withStatus() mutation issue is fixed (now uses a constructor for kImpossible), but withSpeedControl() has the same problem:

public AimParams withSpeedControl(SpeedControl control) {
    this.control = control;
    return this;
}

If anyone calls kImpossible.withSpeedControl(...), it corrupts the shared instance. Low risk in the current code, but the same design hazard.

---

Issue: Spacing inconsistency in ToFAim (ToFAim.java)

for (int i = 0;i < ITERATIONS;i ++) {

Should be for (int i = 0; i < ITERATIONS; i++) — the rest of the codebase (including PhysicsAim) uses consistent spacing.

---

Positives in new commits

• Binary search velocity case now handled with continue — the stall bug from the first pass is fixed
• FuelShotSim.launch() correctly throws if SpeedControl != ProjectileVelocity — good defensive check
• Drag restored in FuelShotSim
• findCC algorithm in Turret is well-implemented with proper bounds checking and tests
• ToFAim is a clean design — iterative time-of-flight compensation is the right approach for empirical aim

[dkbrown8]

Code Review

Overall this is a solid PR. The 3D turret support, constrained SOTM, and ToF recursion are well-structured, and several pre-existing bugs get fixed along the way. A few things worth looking at before merging.

---

Bugs / Correctness

AimParams.kImpossible is a mutable singleton

PhysicsAim.update() assigns AimParams best = AimParams.kImpossible and then returns it directly when no solution is found. Since all fields on AimParams are public and mutable, any caller that receives the singleton and writes to it would corrupt the shared state. The code paths here happen to be safe today (the status is only written to a local output after best = output), but it's fragile. Consider making kImpossible a factory method returning a new instance, or making the fields final.

Typo in Constants.java comment

/** The length of the allianze zone, corresponds to X axis */

"allianze" → "alliance"

TurretTest.turretTest() — forwards() assertion may not exercise findCC()

CommandScheduler.getInstance().schedule(turret.forwards());
verify(mockIO).setPosition(TurretConstants.kForwards);

forwards() calls setPosition(kForwards, false) which passes the value through findCC() first. The mock's inputs.position defaults to 0, so the result depends on kForwards's value. If kForwards is 0, findCC returns 0 and the verify passes trivially without testing the wrapping logic. Worth double-checking the mock setup or verifying with a non-zero initial position.

---

Design Concerns

Hardcoded aiming constraints in StateManager

private final AimStrategy aim = new PhysicsAim(
    new AimConstraints(Rotation2d.fromDegrees(49.5), Rotation2d.fromDegrees(72.0), 18),
    2, 10);

These are tuning values that will need to change on the actual robot. Having them buried in StateManager rather than in TurretConstants/ShooterConstants makes them easy to miss.

FuelShotSim will throw at runtime if strategy is ever switched

if (params.control != SpeedControl.ProjectileVelocity) {
    throw new IllegalStateException(...);
}

This is a reasonable guard for now, but if anyone ever plugs in ToFAim, the simulation will crash without a clear path forward. A comment noting the constraint (or a soft fallback) would be helpful.

AimParams.deltaPitch tolerance widened from 2° to 4°

The diff shows this changed silently. Is 4° the right value? That's a meaningful relaxation of the "ready to shoot" condition.

---

Nitpick

PhysicsAim.update() has a dead if (!ok) branch after the pitch checks that is unreachable — the outer if (ok) already continues, and the two pitch checks cover the only other cases, so the trailing if (!ok) never fires:

if (ok) { ...; continue; }
if (pitch > max) { ...; continue; }
if (pitch < min) { ...; continue; }
if (!ok) { // always true here, but redundant
    upper = guess;
}

---

What's good

• AimStrategy refactored to a pure interface (no mutable params field, no StateManager dependency) — much cleaner
• FieldUtils.allianceRelativeFlip() was returning null for Red alliance before; that's a real bug fix
• Robot.java ordering fix (superstructure.periodic() before CommandScheduler.run()) is important for correct lazy evaluation
• Motor ID fixes (intake/indexer/climber were all 60)
• Lazy evaluation of aim params (Unchecked pattern) is the right approach
• predictedRobotPose() / predictedRobotVelocity() in Drivetrain are clean one-frame lookaheads
• Good test coverage for PhysicsAim and findCC()

[bczdog]

Nice code. Can we predict where a shot might land, and if that coordinate is outside the field, set Impossible?

[therekrab]

Can we predict where a shot might land, and if that coordinate is outside the field, set Impossible?

I don't think that really applies here; the input to any AimStrategy is the goal position; i.e. either the hub or the feed location. When we calculate the aim parameters, we do so for that target, to the best of our knowledge; if we can successfully generate a shot that fits our constraints, we will always believe that the shot we be in the field, because we (more specifically) believe that the shot will land exactly where we need it to.

We set up the problem as one where the end location is specified, so there's no sense in checking the end target. If we pass in an invalid location, something else is being problematic and that's the real issue.