From d6946374205f4bb12e77900b1166f7a1c46d99b9 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Thu, 26 Mar 2026 11:46:33 -0700 Subject: [PATCH 01/10] Adding in polartaingTreeMap --- .../pose/TrajectoriesCalculation.java | 69 ++++++++----------- 1 file changed, 30 insertions(+), 39 deletions(-) diff --git a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java index 605dded3..dbc9e982 100644 --- a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java +++ b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java @@ -13,6 +13,10 @@ //library used for JSON import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.InverseInterpolator; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -31,6 +35,8 @@ import xbot.common.properties.PropertyFactory; import xbot.common.properties.StringProperty; +import static edu.wpi.first.units.Units.RPM; + @Singleton() public class TrajectoriesCalculation { private static final Transform2d HOOD_OFFSET_FROM_CENTER_ROBOT = new Transform2d(Units.Inches.of(-23.5), @@ -94,22 +100,31 @@ public record PresetShootingData(AngularVelocity shooterRPM, double hoodServoRat private record PresetShootingProperties(DoubleProperty shooterRpmProperty, DoubleProperty hoodServoRatio) { } - private static final ShootingData emptyShootingData = new ShootingData(Rotation2d.kZero, Units.RPM.of(0), 0.0); + private static final ShootingData emptyShootingData = new ShootingData(Rotation2d.kZero, RPM.of(0), 0.0); private record TrajectoryKey(double distance) { } // hashmap holding all the values - private static HashMap trajectoryMap = null; + private static InterpolatingTreeMap< Double , HoodTrajectory> trajectoryMap = null; // CHECKSTYLE:OFF // essentially holds all the values for the JSON to fill hashmap - public static class HoodTrajectory { + public static class HoodTrajectory implements Interpolatable { public double distance; public double theta; public double servo; public double velocity; public double RPM; + + @Override + public HoodTrajectory interpolate(HoodTrajectory endValue, double t) { + HoodTrajectory result = new HoodTrajectory(); + result.RPM = this.RPM + (endValue.RPM - this.RPM) * t; // Point slope equation + result.servo = this.servo + (endValue.servo - this.servo) * t; // Point slope equation + return result; + + } } // CHECKSTYLE:ON @@ -147,8 +162,8 @@ public PresetShootingData getPresetShootingSettings(PresetShootingDistance shoot var mapLookup = presetShootingLookup.get(shootingDistance); return new PresetShootingData( - Units.RPM.of(mapLookup.shooterRpmProperty.get()), - mapLookup.hoodServoRatio.get()); + RPM.of(mapLookup.shooterRpmProperty.get()), + mapLookup.hoodServoRatio.get()); } private ShootingData calculateTrajectory(Pose2d robotPose, Pose2d targetPose) { @@ -171,7 +186,7 @@ private Rotation2d rotationToShootFrom(Pose2d robotPose, Pose2d targetPose) { // Fixed shooter parameters. Should only be used when things to very wrong. private ShootingData calculateTrajectoryV1DumbFixedArcToHub(Pose2d robotPose, Pose2d targetPose) { - return new ShootingData(this.rotationToShootFrom(robotPose, targetPose), Units.RPM.of(3800), 0.2); + return new ShootingData(this.rotationToShootFrom(robotPose, targetPose), RPM.of(3800), 0.2); } // Look up known distances based on a preset distances rather than continuous @@ -198,37 +213,18 @@ private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targe Pose2d shooterPose = finalPose.plus(HOOD_OFFSET_FROM_CENTER_ROBOT); double distance = shooterPose.getTranslation().getDistance(targetPose.getTranslation()); - var roundedDistance = Math.round(distance * 100.0) / 100.0; - var offsetDistance = roundedDistance + v3DistanceOffsetMeters.get(); - var key = new TrajectoryKey(offsetDistance); - var hoodTrajectory = this.searchForHoodTrajectory(key); - if (hoodTrajectory.isEmpty()) { - log.warn( - "Trajectory not found, potentially trajectories.json not found or the value doesn't exist in trajectories for distance."); + var offsetDistance = distance + v3DistanceOffsetMeters.get(); + + HoodTrajectory result = trajectoryMap.get(offsetDistance); + if (result == null) { + log.warn("Trajectory not found for distance: {}", offsetDistance); return TrajectoriesCalculation.emptyShootingData; } - var matchedTrajectory = hoodTrajectory.get(); + return new ShootingData(finalRotation, Units.RPM.of(result.RPM), result.servo); - return new ShootingData(finalRotation, Units.RPM.of(matchedTrajectory.RPM), 0); } - private Optional searchForHoodTrajectory(TrajectoryKey key) { - if (trajectoryMap.containsKey(key)) { - return Optional.of(trajectoryMap.get(key)); - } - var adjustedDistanceCheck = key.distance + 0.01; - while (adjustedDistanceCheck < 10.0) { - var check = new TrajectoryKey(adjustedDistanceCheck); - if (trajectoryMap.containsKey(check)) { - return Optional.of(trajectoryMap.get(key)); - } - } - - log.error( - "Trajectory not found, potentially trajectories.json not found or the value doesn't exist in trajectories!"); - return Optional.empty(); - } private record PresetShootingDistanceLookup(Distance distance, PresetShootingDistance presetShootingDistance) { } @@ -253,7 +249,7 @@ private PresetShootingDistance getPresetShootingDistance(Distance distance) { // This method loads the trajectories from the JSON file and populates the // HashMap. private void loadTrajectories() { - trajectoryMap = new HashMap<>(); + trajectoryMap = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Interpolatable::interpolate); try { File configFile = new File(Filesystem.getDeployDirectory(), "trajectories.json"); @@ -264,14 +260,9 @@ private void loadTrajectories() { HoodTrajectory[] rawArray = mapper.readValue(configFile, HoodTrajectory[].class); for (HoodTrajectory point : rawArray) { - var roundedDistance = Math.round(point.distance * 100.0) / 100.0; - var key = new TrajectoryKey(roundedDistance); - if (!trajectoryMap.containsKey(key)) { - trajectoryMap.put(key, point); - } + double key = Math.round(point.distance * 100.0) / 100.0; + trajectoryMap.put(key, point); } - - log.info("Loaded {} trajectories into HashMap.", trajectoryMap.size()); } else { log.warn("Trajectories.json not found in the deploy directory!"); } From 719c294e4bbffee5943677d7159dfef3eac09eac Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Thu, 26 Mar 2026 20:32:23 -0700 Subject: [PATCH 02/10] Unit tests added --- .../components/BaseRobotComponent.java | 4 + .../shooter/TrajectoriesCalculationTest.java | 83 +++++++++++++++++++ 2 files changed, 87 insertions(+) create mode 100644 src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 7a87387e..60f74c70 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/src/main/java/competition/injection/components/BaseRobotComponent.java @@ -11,6 +11,7 @@ import competition.subsystems.hopper_roller.HopperRollerSubsystem; import competition.subsystems.intake_deploy.IntakeDeploySubsystem; import competition.subsystems.lights.LightsSubsystem; +import competition.subsystems.pose.TrajectoriesCalculation; import competition.subsystems.shooter.ShooterSubsystem; import competition.subsystems.shooter.commands.WhenShooterReadyRumbleCommand; import competition.subsystems.shooter_feeder.ShooterFeederSubsystem; @@ -59,4 +60,7 @@ public abstract class BaseRobotComponent extends BaseComponent { public abstract OperatorInterface operatorInterface(); public abstract WhenShooterReadyRumbleCommand whenShooterReadyRumbleCommand(); + + public abstract TrajectoriesCalculation trajectoriesCalculation(); + } diff --git a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java new file mode 100644 index 00000000..2d46acfb --- /dev/null +++ b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java @@ -0,0 +1,83 @@ +package competition.subsystems.shooter; + +import competition.BaseCompetitionTest; +import competition.subsystems.pose.Landmarks; +import competition.subsystems.pose.TrajectoriesCalculation; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.DriverStation; +import org.junit.Test; + +import static org.junit.Assert.assertNotNull; +import static org.junit.Assert.assertTrue; + +public class TrajectoriesCalculationTest extends BaseCompetitionTest { + + public Pose2d hub = Landmarks.getAllianceHubPose( + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded), DriverStation.Alliance.Blue); + + @Test + public void testTrajectoriesCalculation() { + TrajectoriesCalculation calc = getInjectorComponent().trajectoriesCalculation(); + + // Test that we get valid shooting data back at a known distance + Pose2d robotPose = new Pose2d(2.0, 0.0, new Rotation2d(0)); + TrajectoriesCalculation.ShootingData data = calc.calculateAllianceHubShootingData(robotPose); + + // Make sure we got something back and not empty/zero data + assertNotNull(data); + assertTrue("RPM should be greater than 0", data.shooterRPM().in(Units.RPM) > 0); + } + + @Test + public void testInterpolationBetweenTwoPoints() { + TrajectoriesCalculation calc = getInjectorComponent().trajectoriesCalculation(); + + // Use distances in the JSON range (3.797 to 6.105) + Pose2d closePose = new Pose2d(hub.getX() + 3.8, hub.getY(), new Rotation2d(0)); + Pose2d farPose = new Pose2d(hub.getX() + 6.0, hub.getY(), new Rotation2d(0)); + Pose2d midPose = new Pose2d(hub.getX() + 4.9, hub.getY(), new Rotation2d(0)); + + TrajectoriesCalculation.ShootingData closeData = calc.calculateAllianceHubShootingData(closePose); + TrajectoriesCalculation.ShootingData farData = calc.calculateAllianceHubShootingData(farPose); + TrajectoriesCalculation.ShootingData midData = calc.calculateAllianceHubShootingData(midPose); + + double closeServo = closeData.servoRatio(); + double farServo = farData.servoRatio(); + double midServo = midData.servoRatio(); + + // Servo increases with distance so mid should be between close and far + assertTrue("Interpolated servo should be between close and far", + midServo > closeServo && midServo < farServo); + } + + @Test + public void testTenRandomPointsInAllianceZone() { + TrajectoriesCalculation calc = getInjectorComponent().trajectoriesCalculation(); + + for (int i = 0; i < 10; i++){ + + double randomWidth = (Math.random() * 4.03); // + double randomHeight = (Math.random() * 8.07); + Pose2d randomPose = new Pose2d(randomWidth, randomHeight, new Rotation2d(0)); + + TrajectoriesCalculation.ShootingData data = calc.calculateAllianceHubShootingData(randomPose); + + assertNotNull(data); + + if (data.servoRatio() > 0) { + assertTrue("Servo should be between 0.2 and 1.0 to see if it goes ni", + data.servoRatio() >= 0.2 && data.servoRatio() <= 1.0); + } + + } + + } + + +} + + From 1c1f9dd075c0b88d65aed98bd13a64fc66945c6b Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 28 Mar 2026 10:11:08 -0700 Subject: [PATCH 03/10] Comment changes --- .../subsystems/shooter/TrajectoriesCalculationTest.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java index 2d46acfb..b660e267 100644 --- a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java +++ b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java @@ -60,16 +60,16 @@ public void testTenRandomPointsInAllianceZone() { for (int i = 0; i < 10; i++){ - double randomWidth = (Math.random() * 4.03); // - double randomHeight = (Math.random() * 8.07); - Pose2d randomPose = new Pose2d(randomWidth, randomHeight, new Rotation2d(0)); + double randomWidth = (Math.random() * 4.03); // 4.03 is the width of the alliance zone + double randomHeight = (Math.random() * 8.07); // 8.07 is the height of the alliance zone + Pose2d randomPose = new Pose2d(randomWidth, randomHeight, new Rotation2d(0)); // Random spot in alliance zone TrajectoriesCalculation.ShootingData data = calc.calculateAllianceHubShootingData(randomPose); assertNotNull(data); if (data.servoRatio() > 0) { - assertTrue("Servo should be between 0.2 and 1.0 to see if it goes ni", + assertTrue("Servo should be between 0.2 and 1.0", data.servoRatio() >= 0.2 && data.servoRatio() <= 1.0); } From d3e4748586b87d8e0472c3767b483aa181296aba Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 28 Mar 2026 11:15:37 -0700 Subject: [PATCH 04/10] file name change --- .../competition/subsystems/pose/TrajectoriesCalculation.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java index 66bd7c2a..b4b19d1e 100644 --- a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java +++ b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java @@ -253,7 +253,7 @@ private void loadTrajectories() { try { // TODO: Needs to be merged with https://github.com/Team488/TeamXbot2026/pull/309/ which includes the ability to do no hood and hood values. - File configFile = new File(Filesystem.getDeployDirectory(), "trajectories_0_hood.json"); + File configFile = new File(Filesystem.getDeployDirectory(), "trajectories.json"); if (configFile.exists()) { ObjectMapper mapper = new ObjectMapper(); From ebf0f339380c0708d3a1796523d332fbb1ca5a13 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 28 Mar 2026 11:18:44 -0700 Subject: [PATCH 05/10] Re-added TrajectoryCalc in BaseRobotCompponent --- .../competition/injection/components/BaseRobotComponent.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 0daa6f82..139376fb 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/src/main/java/competition/injection/components/BaseRobotComponent.java @@ -11,6 +11,7 @@ import competition.subsystems.hopper_roller.HopperRollerSubsystem; import competition.subsystems.intake_deploy.IntakeDeploySubsystem; import competition.subsystems.lights.LightsSubsystem; +import competition.subsystems.pose.TrajectoriesCalculation; import competition.subsystems.shooter.ShooterSubsystem; import competition.subsystems.shooter.commands.WhenShooterReadyRumbleCommand; import competition.subsystems.shooter_feeder.ShooterFeederSubsystem; @@ -61,5 +62,7 @@ public abstract class BaseRobotComponent extends BaseComponent { public abstract WhenShooterReadyRumbleCommand whenShooterReadyRumbleCommand(); + public abstract TrajectoriesCalculation trajectoriesCalculation(); + public abstract SuperstructureMechanismSubsystem superstructureMechanismSubsystem(); } From 4f30de1fd65a93c4dbe68020cc69fb5b18a62df0 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 28 Mar 2026 14:04:23 -0700 Subject: [PATCH 06/10] Extra spacing removed --- .../subsystems/shooter/TrajectoriesCalculationTest.java | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java index b660e267..cd24ac0d 100644 --- a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java +++ b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java @@ -72,12 +72,6 @@ public void testTenRandomPointsInAllianceZone() { assertTrue("Servo should be between 0.2 and 1.0", data.servoRatio() >= 0.2 && data.servoRatio() <= 1.0); } - } - } - - -} - - +} \ No newline at end of file From e896fcb5e3b90baf438a1c2aeb56f442f2f374d9 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 28 Mar 2026 15:31:21 -0700 Subject: [PATCH 07/10] Brining back interpolatingTreeMap stuff --- .../pose/TrajectoriesCalculation.java | 74 +++++++++---------- 1 file changed, 35 insertions(+), 39 deletions(-) diff --git a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java index b0b9d95e..b592a655 100644 --- a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java +++ b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java @@ -13,6 +13,10 @@ //library used for JSON import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.Interpolator; +import edu.wpi.first.math.interpolation.InverseInterpolator; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -31,6 +35,8 @@ import xbot.common.properties.PropertyFactory; import xbot.common.properties.StringProperty; +import static edu.wpi.first.units.Units.RPM; + @Singleton() public class TrajectoriesCalculation { private static final Transform2d HOOD_OFFSET_FROM_CENTER_ROBOT = new Transform2d(Units.Inches.of(-23.5), @@ -94,24 +100,34 @@ public record PresetShootingData(AngularVelocity shooterRPM, double hoodServoRat private record PresetShootingProperties(DoubleProperty shooterRpmProperty, DoubleProperty hoodServoRatio) { } - private static final ShootingData emptyShootingData = new ShootingData(Rotation2d.kZero, Units.RPM.of(0), 0.0); + private static final ShootingData emptyShootingData = new ShootingData(Rotation2d.kZero, RPM.of(0), 0.0); private record TrajectoryKey(double distance) { } // hashmaps holding all the values - private static HashMap trajectoryMap = null; - private static HashMap trajectoryZeroHoodMap = null; + private static InterpolatingTreeMap trajectoryMap = null; + private static InterpolatingTreeMap trajectoryZeroHoodMap = null; // CHECKSTYLE:OFF // essentially holds all the values for the JSON to fill hashmap - public static class HoodTrajectory { + public static class HoodTrajectory implements Interpolatable { public double distance; public double theta; public double servo; public double velocity; public double RPM; + + @Override + public HoodTrajectory interpolate(HoodTrajectory endValue, double t) { + HoodTrajectory result = new HoodTrajectory(); + result.RPM = this.RPM + (endValue.RPM - this.RPM) * t; // Point slope equation + result.servo = this.servo + (endValue.servo - this.servo) * t; // Point slope equation + + return result; + } } + // CHECKSTYLE:ON public enum PresetShootingDistance { @@ -148,8 +164,8 @@ public PresetShootingData getPresetShootingSettings(PresetShootingDistance shoot var mapLookup = presetShootingLookup.get(shootingDistance); return new PresetShootingData( - Units.RPM.of(mapLookup.shooterRpmProperty.get()), - mapLookup.hoodServoRatio.get()); + RPM.of(mapLookup.shooterRpmProperty.get()), + mapLookup.hoodServoRatio.get()); } private ShootingData calculateTrajectory(Pose2d robotPose, Pose2d targetPose, boolean zeroHood) { @@ -168,7 +184,7 @@ private Rotation2d rotationToShootFrom(Pose2d robotPose, Pose2d targetPose) { // Fixed shooter parameters. Should only be used when things to very wrong. private ShootingData calculateTrajectoryV1DumbFixedArcToHub(Pose2d robotPose, Pose2d targetPose) { - return new ShootingData(this.rotationToShootFrom(robotPose, targetPose), Units.RPM.of(3800), 0.2); + return new ShootingData(this.rotationToShootFrom(robotPose, targetPose), RPM.of(3800), 0.2); } // Look up known distances based on a preset distances rather than continuous @@ -202,40 +218,23 @@ private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targe Pose2d shooterPose = finalPose.plus(HOOD_OFFSET_FROM_CENTER_ROBOT); double distance = shooterPose.getTranslation().getDistance(targetPose.getTranslation()); - var roundedDistance = Math.round(distance * 100.0) / 100.0; - var offsetDistance = roundedDistance + v3DistanceOffsetMeters.get(); + var offsetDistance = distance + v3DistanceOffsetMeters.get(); var key = new TrajectoryKey(offsetDistance); var hoodTrajectory = this.searchForHoodTrajectory(key, zeroHood); - if (hoodTrajectory.isEmpty()) { - if (zeroHood) { - log.warn( - "Trajectory not found, potentially trajectories_0_hood.json not found or the value doesn't exist in trajectories for distance."); - } else { - log.warn( - "Trajectory not found, potentially trajectories.json not found or the value doesn't exist in trajectories for distance."); - } + HoodTrajectory result = trajectoryMap.get(key); + if (result == null) { + log.warn("Trajectory not found for distance: {}", offsetDistance); return TrajectoriesCalculation.emptyShootingData; } var matchedTrajectory = hoodTrajectory.get(); - return new ShootingData(finalRotation, Units.RPM.of(matchedTrajectory.RPM), 0); // Use matchedTrajectory.servo instead of 0 if we are using the hood + return new ShootingData(finalRotation, Units.RPM.of(result.RPM), result.servo); // Use matchedTrajectory.servo instead of 0 if we are using the hood } private Optional searchForHoodTrajectory(TrajectoryKey key, boolean zeroHood) { var mapToUse = zeroHood ? trajectoryZeroHoodMap : trajectoryMap; - if (mapToUse.containsKey(key)) { - return Optional.of(mapToUse.get(key)); - } - var adjustedDistanceCheck = key.distance + 0.01; - while (adjustedDistanceCheck < 10.0) { - var check = new TrajectoryKey(adjustedDistanceCheck); - if (mapToUse.containsKey(check)) { - return Optional.of(mapToUse.get(check)); - } - adjustedDistanceCheck = key.distance + 0.01; - } if (zeroHood) { log.error( @@ -269,8 +268,8 @@ private PresetShootingDistance getPresetShootingDistance(Distance distance) { // This method loads the trajectories from the JSON file and populates the // HashMap. - private HashMap loadTrajectories(String filename) { - var loadedMap = new HashMap(); + private InterpolatingTreeMap loadTrajectories(String filename) { + try { File configFile = new File(Filesystem.getDeployDirectory(), filename); @@ -281,14 +280,11 @@ private HashMap loadTrajectories(String filename) HoodTrajectory[] rawArray = mapper.readValue(configFile, HoodTrajectory[].class); for (HoodTrajectory point : rawArray) { - var roundedDistance = Math.round(point.distance * 100.0) / 100.0; - var key = new TrajectoryKey(roundedDistance); - if (!loadedMap.containsKey(key)) { - loadedMap.put(key, point); - } + TrajectoryKey key = new TrajectoryKey(point.distance); + trajectoryMap.put(key, point); } - log.info("Loaded {} trajectories from {} into HashMap.", loadedMap.size(), filename); + log.info("Loaded {} trajectories from {} into the TreeMap.", filename); } else { log.warn("{} not found in the deploy directory!", filename); } @@ -296,6 +292,6 @@ private HashMap loadTrajectories(String filename) log.error("Failed to load JSON from {}: {}", filename, e.getMessage()); } - return loadedMap; + return trajectoryMap; } -} +} \ No newline at end of file From 9b95024c20c0c49ae523f945b7f64a2e724a705e Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 28 Mar 2026 16:59:09 -0700 Subject: [PATCH 08/10] Restore InterpolatingTreeMap --- .../pose/TrajectoriesCalculation.java | 52 ++++------- .../shooter/TrajectoriesCalculationTest.java | 92 +++++++++++++------ 2 files changed, 82 insertions(+), 62 deletions(-) diff --git a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java index b592a655..a692ab59 100644 --- a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java +++ b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java @@ -106,8 +106,8 @@ private record TrajectoryKey(double distance) { } // hashmaps holding all the values - private static InterpolatingTreeMap trajectoryMap = null; - private static InterpolatingTreeMap trajectoryZeroHoodMap = null; + private static InterpolatingTreeMap trajectoryMap; + private static InterpolatingTreeMap trajectoryZeroHoodMap; // CHECKSTYLE:OFF // essentially holds all the values for the JSON to fill hashmap @@ -203,38 +203,30 @@ private ShootingData calculateTrajectoryV2KnownDistance(Pose2d robotPose, Pose2d // Look up optimal shooting parameters based on current pose and shooting // target's pose. private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targetPose, boolean zeroHood) { - // This does take a performance hit, so we should consider loading these in when we aren't doing anything. - if (zeroHood) { - if (trajectoryZeroHoodMap == null) { - trajectoryZeroHoodMap = loadTrajectories("trajectories_0_hood.json"); - } - } else { - if (trajectoryMap == null) { - trajectoryMap = loadTrajectories("trajectories.json"); - } + if (trajectoryMap == null) { + trajectoryMap = loadTrajectories("trajectories.json"); + } + if (trajectoryZeroHoodMap == null) { + trajectoryZeroHoodMap = loadTrajectories("trajectories_0_hood.json"); } + + InterpolatingTreeMap activeMap = zeroHood ? trajectoryZeroHoodMap : trajectoryMap; + Rotation2d finalRotation = this.rotationToShootFrom(robotPose, targetPose); Pose2d finalPose = new Pose2d(robotPose.getX(), robotPose.getY(), finalRotation); - Pose2d shooterPose = finalPose.plus(HOOD_OFFSET_FROM_CENTER_ROBOT); double distance = shooterPose.getTranslation().getDistance(targetPose.getTranslation()); var offsetDistance = distance + v3DistanceOffsetMeters.get(); - var key = new TrajectoryKey(offsetDistance); - var hoodTrajectory = this.searchForHoodTrajectory(key, zeroHood); - HoodTrajectory result = trajectoryMap.get(key); + + HoodTrajectory result = activeMap.get(offsetDistance); if (result == null) { log.warn("Trajectory not found for distance: {}", offsetDistance); return TrajectoriesCalculation.emptyShootingData; } - var matchedTrajectory = hoodTrajectory.get(); - - return new ShootingData(finalRotation, Units.RPM.of(result.RPM), result.servo); // Use matchedTrajectory.servo instead of 0 if we are using the hood + return new ShootingData(finalRotation, Units.RPM.of(result.RPM), result.servo); } private Optional searchForHoodTrajectory(TrajectoryKey key, boolean zeroHood) { - var mapToUse = zeroHood ? trajectoryZeroHoodMap : trajectoryMap; - - if (zeroHood) { log.error( @@ -268,30 +260,24 @@ private PresetShootingDistance getPresetShootingDistance(Distance distance) { // This method loads the trajectories from the JSON file and populates the // HashMap. - private InterpolatingTreeMap loadTrajectories(String filename) { - - + private InterpolatingTreeMap loadTrajectories(String filename) { + InterpolatingTreeMap map = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Interpolatable::interpolate); try { File configFile = new File(Filesystem.getDeployDirectory(), filename); - if (configFile.exists()) { ObjectMapper mapper = new ObjectMapper(); - HoodTrajectory[] rawArray = mapper.readValue(configFile, HoodTrajectory[].class); - for (HoodTrajectory point : rawArray) { - TrajectoryKey key = new TrajectoryKey(point.distance); - trajectoryMap.put(key, point); + map.put(point.distance, point); } - - log.info("Loaded {} trajectories from {} into the TreeMap.", filename); + log.info("Loaded trajectories from {}", filename); } else { log.warn("{} not found in the deploy directory!", filename); } } catch (Exception e) { log.error("Failed to load JSON from {}: {}", filename, e.getMessage()); } - - return trajectoryMap; + return map; // always returns map, even if empty } } \ No newline at end of file diff --git a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java index cd24ac0d..fc3659eb 100644 --- a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java +++ b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj.DriverStation; import org.junit.Test; +import java.util.Random; + import static org.junit.Assert.assertNotNull; import static org.junit.Assert.assertTrue; @@ -19,58 +21,90 @@ public class TrajectoriesCalculationTest extends BaseCompetitionTest { public Pose2d hub = Landmarks.getAllianceHubPose( AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded), DriverStation.Alliance.Blue); + private TrajectoriesCalculation.ShootingData getData( + TrajectoriesCalculation calc, + Pose2d pose, + boolean zeroHood) { + + return calc.calculateAllianceHubShootingData(pose, zeroHood); + } + @Test - public void testTrajectoriesCalculation() { + public void testZeroHoodVsNormal() { TrajectoriesCalculation calc = getInjectorComponent().trajectoriesCalculation(); - // Test that we get valid shooting data back at a known distance - Pose2d robotPose = new Pose2d(2.0, 0.0, new Rotation2d(0)); - TrajectoriesCalculation.ShootingData data = calc.calculateAllianceHubShootingData(robotPose); - // Make sure we got something back and not empty/zero data - assertNotNull(data); - assertTrue("RPM should be greater than 0", data.shooterRPM().in(Units.RPM) > 0); + Pose2d robotPose = new Pose2d(4.0, 2.0, new Rotation2d()); + + var normal = calc.calculateAllianceHubShootingData(robotPose, false); + var zeroHood = calc.calculateAllianceHubShootingData(robotPose, true); + + // Both should return valid data + assertNotNull(normal); + assertNotNull(zeroHood); + + // RPM should exist in both + assertTrue(normal.shooterRPM().in(Units.RPM) > 0); + assertTrue(zeroHood.shooterRPM().in(Units.RPM) > 0); + + assertTrue("Zero hood servo should be <= normal servo", + zeroHood.servoRatio() <= normal.servoRatio()); } @Test public void testInterpolationBetweenTwoPoints() { TrajectoriesCalculation calc = getInjectorComponent().trajectoriesCalculation(); - // Use distances in the JSON range (3.797 to 6.105) - Pose2d closePose = new Pose2d(hub.getX() + 3.8, hub.getY(), new Rotation2d(0)); - Pose2d farPose = new Pose2d(hub.getX() + 6.0, hub.getY(), new Rotation2d(0)); - Pose2d midPose = new Pose2d(hub.getX() + 4.9, hub.getY(), new Rotation2d(0)); + Pose2d closePose = new Pose2d(hub.getX() + 3.8, hub.getY(), new Rotation2d()); + Pose2d midPose = new Pose2d(hub.getX() + 4.9, hub.getY(), new Rotation2d()); + Pose2d farPose = new Pose2d(hub.getX() + 6.0, hub.getY(), new Rotation2d()); + + for (boolean zeroHood : new boolean[]{false, true}) { - TrajectoriesCalculation.ShootingData closeData = calc.calculateAllianceHubShootingData(closePose); - TrajectoriesCalculation.ShootingData farData = calc.calculateAllianceHubShootingData(farPose); - TrajectoriesCalculation.ShootingData midData = calc.calculateAllianceHubShootingData(midPose); + var closeShot = getData(calc, closePose, zeroHood); + var midShot = getData(calc, midPose, zeroHood); + var farShot = getData(calc, farPose, zeroHood); - double closeServo = closeData.servoRatio(); - double farServo = farData.servoRatio(); - double midServo = midData.servoRatio(); + assertNotNull(closeShot); + assertNotNull(midShot); + assertNotNull(farShot); - // Servo increases with distance so mid should be between close and far - assertTrue("Interpolated servo should be between close and far", - midServo > closeServo && midServo < farServo); + if (!zeroHood) { + // Normal mode → servo should interpolate + assertTrue(midShot.servoRatio() > closeShot.servoRatio()); + assertTrue(midShot.servoRatio() < farShot.servoRatio()); + } else { + // Zero hood → servo should NOT change (likely 0) + assertTrue(Math.abs(midShot.servoRatio() - closeShot.servoRatio()) < 0.001); + assertTrue(Math.abs(midShot.servoRatio() - farShot.servoRatio()) < 0.001); + } + } } @Test public void testTenRandomPointsInAllianceZone() { TrajectoriesCalculation calc = getInjectorComponent().trajectoriesCalculation(); - for (int i = 0; i < 10; i++){ + long seed = System.currentTimeMillis(); + Random rand = new Random(seed); + + System.out.println("Seed: " + seed); + + for (boolean zeroHood : new boolean[]{false, true}) { + for (int i = 0; i < 10; i++) { + double randomWidth = rand.nextDouble() * 4.03; // 4.03 is the width of the alliance zone + double randomHeight = rand.nextDouble() * 8.07; // 8.07 is the height of the alliance zone - double randomWidth = (Math.random() * 4.03); // 4.03 is the width of the alliance zone - double randomHeight = (Math.random() * 8.07); // 8.07 is the height of the alliance zone - Pose2d randomPose = new Pose2d(randomWidth, randomHeight, new Rotation2d(0)); // Random spot in alliance zone + Pose2d randomPose = new Pose2d(randomWidth, randomHeight, new Rotation2d(0)); // Random spot in alliance zone - TrajectoriesCalculation.ShootingData data = calc.calculateAllianceHubShootingData(randomPose); + TrajectoriesCalculation.ShootingData data = calc.calculateAllianceHubShootingData(randomPose,zeroHood); - assertNotNull(data); + assertNotNull(data); - if (data.servoRatio() > 0) { - assertTrue("Servo should be between 0.2 and 1.0", - data.servoRatio() >= 0.2 && data.servoRatio() <= 1.0); + if (data.servoRatio() > 0) { + assertTrue("Servo should be between 0.2 and 1.0", + data.servoRatio() >= 0.2 && data.servoRatio() <= 1.0); + } } } } From 7aed2e07a90e7b60ce2f64a2b5b2656c32ca2cc8 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 4 Apr 2026 13:12:31 -0700 Subject: [PATCH 09/10] Less hardcoding, and small test change --- .../shooter/TrajectoriesCalculationTest.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java index fc3659eb..4d017c5c 100644 --- a/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java +++ b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java @@ -10,6 +10,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import org.junit.Test; +import xbot.common.subsystems.pose.GameField; import java.util.Random; @@ -91,19 +92,21 @@ public void testTenRandomPointsInAllianceZone() { System.out.println("Seed: " + seed); for (boolean zeroHood : new boolean[]{false, true}) { + GameField gameField = getInjectorComponent().gameField(); for (int i = 0; i < 10; i++) { - double randomWidth = rand.nextDouble() * 4.03; // 4.03 is the width of the alliance zone - double randomHeight = rand.nextDouble() * 8.07; // 8.07 is the height of the alliance zone + double randomWidth = rand.nextDouble() * gameField.getFieldWidth().in(Units.Meters); // 4.03 is the width of the alliance zone + double randomLength = rand.nextDouble() * gameField.getFieldLength().in(Units.Meters); // 8.07 is the height of the alliance zone - Pose2d randomPose = new Pose2d(randomWidth, randomHeight, new Rotation2d(0)); // Random spot in alliance zone + Pose2d randomPose = new Pose2d(randomWidth, randomLength, new Rotation2d(0)); // Random spot in alliance zone TrajectoriesCalculation.ShootingData data = calc.calculateAllianceHubShootingData(randomPose,zeroHood); assertNotNull(data); + // Servo ratio should be between 0 (No hood) and 1.0 (Max hood) if (data.servoRatio() > 0) { - assertTrue("Servo should be between 0.2 and 1.0", - data.servoRatio() >= 0.2 && data.servoRatio() <= 1.0); + assertTrue("Servo should be between 0.0 and 1.0", + data.servoRatio() <= 1.0); } } } From 20ab66f45dfca26692c1610c7c6afa49a850fd24 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 4 Apr 2026 16:36:42 -0700 Subject: [PATCH 10/10] readded some commands from main --- ...ntinuousPrepareToShootFromHereCommand.java | 2 +- .../pose/TrajectoriesCalculation.java | 100 ++++++++++++++++-- 2 files changed, 95 insertions(+), 7 deletions(-) diff --git a/src/main/java/competition/command_groups/ContinuousPrepareToShootFromHereCommand.java b/src/main/java/competition/command_groups/ContinuousPrepareToShootFromHereCommand.java index c4a351e0..cabba0b1 100644 --- a/src/main/java/competition/command_groups/ContinuousPrepareToShootFromHereCommand.java +++ b/src/main/java/competition/command_groups/ContinuousPrepareToShootFromHereCommand.java @@ -62,7 +62,7 @@ private void prepareToShootAtTarget() { TrajectoriesCalculation.ShootingData data; switch (this.target) { case ALLIANCE_ZONE: - data = this.trajectoriesCalculation.calculateAllianceZoneShootingData(pose, this.zeroHood); + data = this.trajectoriesCalculation.calculateAllianceZoneShootingDataV4(pose, this.zeroHood); break; case HUB: default: diff --git a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java index a692ab59..3cf89273 100644 --- a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java +++ b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.interpolation.InterpolatingTreeMap; -import edu.wpi.first.math.interpolation.Interpolator; import edu.wpi.first.math.interpolation.InverseInterpolator; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -160,6 +159,22 @@ public ShootingData calculateAllianceZoneShootingData(Pose2d robotPose, boolean return calculateTrajectory(robotPose, targetPose, zeroHood); } + public ShootingData calculateAllianceZoneShootingDataV4(Pose2d robotPose, boolean zeroHood) { + Pose2d closestTrenchNeutralSideIdPose = Landmarks.getClosestTrenchNeutralSideIdPose( + aprilTagFieldLayout, + DriverStation.getAlliance().orElse(Alliance.Blue), + robotPose); + + Translation2d target = Landmarks + .getAllianceHubPose(aprilTagFieldLayout, DriverStation.getAlliance().orElse(Alliance.Blue)) + .getTranslation() + .interpolate(closestTrenchNeutralSideIdPose.getTranslation(), interpolationFactor.get()); + + Pose2d targetPose = new Pose2d(target, Rotation2d.kZero); + + return calculateTrajectory(robotPose, targetPose, zeroHood); + } + public PresetShootingData getPresetShootingSettings(PresetShootingDistance shootingDistance) { var mapLookup = presetShootingLookup.get(shootingDistance); @@ -172,7 +187,7 @@ private ShootingData calculateTrajectory(Pose2d robotPose, Pose2d targetPose, bo return switch (trajectoryCalcVersion.get()) { case "1" -> this.calculateTrajectoryV1DumbFixedArcToHub(robotPose, targetPose); case "2" -> this.calculateTrajectoryV2KnownDistance(robotPose, targetPose); - default -> this.calculateTrajectoryV3Dynamic(robotPose, targetPose, zeroHood); + default -> this.calculateTrajectoryV4Dynamic(robotPose, targetPose, zeroHood); }; } @@ -200,14 +215,55 @@ private ShootingData calculateTrajectoryV2KnownDistance(Pose2d robotPose, Pose2d shootingData.hoodServoRatio); } + private double roundingDistance(double distance) { + return Math.round(distance * 100.0) / 100.0; + } + + private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targetPose, boolean zeroHood) { + // This does take a performance hit, so we should consider loading these in when we aren't doing anything. + if (zeroHood) { + if (trajectoryZeroHoodMap == null) { + trajectoryZeroHoodMap = loadTrajectoriesV4("trajectories_0_hood.json"); + } + } else { + if (trajectoryMap == null) { + trajectoryMap = loadTrajectoriesV4("trajectories.json"); + } + } + Rotation2d finalRotation = this.rotationToShootFrom(robotPose, targetPose); + Pose2d finalPose = new Pose2d(robotPose.getX(), robotPose.getY(), finalRotation); + + Pose2d shooterPose = finalPose.plus(HOOD_OFFSET_FROM_CENTER_ROBOT); + double distance = shooterPose.getTranslation().getDistance(targetPose.getTranslation()); + var roundedDistance = roundingDistance(distance); + var offsetDistance = roundedDistance + v3DistanceOffsetMeters.get(); + var key = new TrajectoryKey(roundingDistance(offsetDistance)); + var hoodTrajectory = this.searchForHoodTrajectory(key, zeroHood); + if (hoodTrajectory.isEmpty()) { + if (zeroHood) { + log.warn( + "Trajectory not found, potentially trajectories_0_hood.json not found or the value doesn't exist in trajectories for distance."); + } else { + log.warn( + "Trajectory not found, potentially trajectories.json not found or the value doesn't exist in trajectories for distance."); + } + return TrajectoriesCalculation.emptyShootingData; + } + var matchedTrajectory = hoodTrajectory.get(); + + log.info("Returning shooting speed: {}, servo: {}", matchedTrajectory.RPM, matchedTrajectory.servo); + + return new ShootingData(finalRotation, Units.RPM.of(matchedTrajectory.RPM), matchedTrajectory.servo); + } + // Look up optimal shooting parameters based on current pose and shooting // target's pose. - private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targetPose, boolean zeroHood) { + private ShootingData calculateTrajectoryV4Dynamic(Pose2d robotPose, Pose2d targetPose, boolean zeroHood) { if (trajectoryMap == null) { - trajectoryMap = loadTrajectories("trajectories.json"); + trajectoryMap = loadTrajectoriesV4("trajectories.json"); } if (trajectoryZeroHoodMap == null) { - trajectoryZeroHoodMap = loadTrajectories("trajectories_0_hood.json"); + trajectoryZeroHoodMap = loadTrajectoriesV4("trajectories_0_hood.json"); } InterpolatingTreeMap activeMap = zeroHood ? trajectoryZeroHoodMap : trajectoryMap; @@ -260,7 +316,7 @@ private PresetShootingDistance getPresetShootingDistance(Distance distance) { // This method loads the trajectories from the JSON file and populates the // HashMap. - private InterpolatingTreeMap loadTrajectories(String filename) { + private InterpolatingTreeMap loadTrajectoriesV4(String filename) { InterpolatingTreeMap map = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Interpolatable::interpolate); try { @@ -280,4 +336,36 @@ private InterpolatingTreeMap loadTrajectories(String fil } return map; // always returns map, even if empty } + + private HashMap loadTrajectories(String filename) { + var loadedMap = new HashMap(); + + try { + File configFile = new File(Filesystem.getDeployDirectory(), filename); + + if (configFile.exists()) { + ObjectMapper mapper = new ObjectMapper(); + + HoodTrajectory[] rawArray = mapper.readValue(configFile, HoodTrajectory[].class); + + for (HoodTrajectory point : rawArray) { + var roundedDistance = Math.round(point.distance * 100.0) / 100.0; + var key = new TrajectoryKey(roundedDistance); + if (!loadedMap.containsKey(key)) { + loadedMap.put(key, point); + } + } + + log.info("Loaded {} trajectories from {} into HashMap.", loadedMap.size(), filename); + } else { + log.warn("{} not found in the deploy directory!", filename); + } + } catch (Exception e) { + log.error("Failed to load JSON from {}: {}", filename, e.getMessage()); + } + + return loadedMap; + } + + } \ No newline at end of file