From 47afe8aaa59318d45d1192958ac4ea50e3f5c91c Mon Sep 17 00:00:00 2001 From: iefomit Date: Thu, 2 Apr 2026 22:06:25 -0700 Subject: [PATCH] tolerance to reduce iterations --- .../robot/commands/gpm/Superstructure.java | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index cf16f9a..3b3fe90 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -102,30 +102,38 @@ public class Superstructure extends Command { Pose2d lookaheadPose = turretPosition; /* - * Loop (20) until lookaheadPose converges BECAUSE --> + * Loop (max 20) until lookaheadPose converges BECAUSE --> * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s) * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s * So we make a bunch of guesses until it converges + * Early exit when change < 1mm to avoid unnecessary iterations */ for (int i = 0; i < 20; i++) { Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); - Translation3d target3d = new Translation3d(target.getX(), target.getY(), + Translation3d target3d = new Translation3d(target.getX(), target.getY(), target == FieldConstants.getHubTranslation().toTranslation2d() ? FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub goalState = ShooterPhysics.getShotParams( - Translation2d.kZero, - target3d.minus(lookahead3d), - 2.0); + Translation2d.kZero, + target3d.minus(lookahead3d), + 2.0); timeOfFlight = goalState.timeOfFlight() * TOFAdjustment; double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; - lookaheadPose = + Pose2d newLookaheadPose = new Pose2d( turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), turretPosition.getRotation()); + + // early exit if converged (change < 1mm) + if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) { + lookaheadPose = newLookaheadPose; + break; + } + lookaheadPose = newLookaheadPose; } // Get the field angle relative to the target pose -- 2.39.5