From 89dcb54891055b6ced51a7677de6a1980f74cba4 Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Sun, 19 Apr 2026 10:48:31 -0700 Subject: [PATCH] shuttling tuned (seperate non dynamic TOF for shuttling) still needs last interpolation key --- .../frc/robot/commands/gpm/Superstructure.java | 15 +++++++++++++-- .../frc/robot/constants/ShuttleInterpolation.java | 11 +++++------ 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 27fb45b..d767fe8 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -66,6 +66,7 @@ public class Superstructure extends Command { private double turretOffset = 0.0; private double distanceFromTarget = 0.0; + private double shuttlingTOFMultiplier = 0.8; // private double TOFAdjustment = 0.85; // private double TOFAdjustment = 1.1; @@ -111,11 +112,12 @@ public class Superstructure extends Command { * So we make a bunch of guesses until it converges * Early exit when change < 1mm to avoid unnecessary iterations */ + boolean shuttling = target.equals(FieldConstants.getHubTranslation().toTranslation2d()); 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(), - target.equals(FieldConstants.getHubTranslation().toTranslation2d()) ? + shuttling ? FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub goalState = ShooterPhysics.getShotParams( @@ -123,7 +125,13 @@ public class Superstructure extends Command { target3d.minus(lookahead3d), 2.0); - timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get(); + if (!shuttling) { + timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get(); + } else { + double distance = target.getDistance(lookaheadPose.getTranslation()); + timeOfFlight = distance * shuttlingTOFMultiplier; + } + double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; Pose2d newLookaheadPose = @@ -251,6 +259,9 @@ public class Superstructure extends Command { turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset); SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset); + shuttlingTOFMultiplier = SmartDashboard.getNumber("Shuttling TOF Multiplier", shuttlingTOFMultiplier); + SmartDashboard.putNumber("Shuttling TOF Multiplier", shuttlingTOFMultiplier); + if (phaseManager.isIdle()) { underLadder(); } else { diff --git a/src/main/java/frc/robot/constants/ShuttleInterpolation.java b/src/main/java/frc/robot/constants/ShuttleInterpolation.java index 3d8074e..873db71 100644 --- a/src/main/java/frc/robot/constants/ShuttleInterpolation.java +++ b/src/main/java/frc/robot/constants/ShuttleInterpolation.java @@ -19,13 +19,12 @@ public class ShuttleInterpolation { // we can be less aggressive: y = 0.65 * (1.34959x + 9.79618) // will likely be this that requires tuning. shooterVelocityMap.put(0.0, 9.0); - shooterVelocityMap.put(4.0, 12.8); - shooterVelocityMap.put(7.6, 19.0); - shooterVelocityMap.put(11.4, 28.1); // was 25.2 before - shooterVelocityMap.put(16.54, 44.8); // untested + shooterVelocityMap.put(4.0, 12.0 * 1.3); // tuned + shooterVelocityMap.put(8.0, 22.0 * 1.075); // tuned + shooterVelocityMap.put(16.0, 44.0); // untuned // always shoot at low angle to ground. - newHoodMap.put(0.0, 60.0); - newHoodMap.put(27.99, 60.0); + newHoodMap.put(0.0, 55.0); + newHoodMap.put(27.99, 55.0); } } -- 2.39.5