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/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(); } diff --git a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java index f05e9dfe..3cf89273 100644 --- a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java +++ b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java @@ -13,6 +13,9 @@ //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.InverseInterpolator; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -27,11 +30,12 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; -import xbot.common.advantage.AKitLogger; import xbot.common.properties.DoubleProperty; 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), @@ -44,7 +48,6 @@ public class TrajectoriesCalculation { private final DoubleProperty interpolationFactor; private final DoubleProperty v3DistanceOffsetMeters; private final StringProperty trajectoryCalcVersion; - private AKitLogger aKitLog; private static void getOrCreatePresetLookup(PropertyFactory propManager) { if (presetShootingLookup == null) { @@ -74,7 +77,6 @@ private static void getOrCreatePresetLookup(PropertyFactory propManager) { @Inject public TrajectoriesCalculation(AprilTagFieldLayout aprilTagFieldLayout, PropertyFactory propManager) { - this.aKitLog = new AKitLogger("TrajectoriesCalculation"); this.aprilTagFieldLayout = aprilTagFieldLayout; this.log = LogManager.getLogger(getClass().getName()); propManager.setPrefix("TrajectoriesCalculation"); @@ -97,24 +99,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; + private static InterpolatingTreeMap trajectoryZeroHoodMap; // 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 { @@ -147,19 +159,35 @@ 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); 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) { 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); }; } @@ -171,7 +199,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 @@ -191,17 +219,15 @@ private double roundingDistance(double distance) { return Math.round(distance * 100.0) / 100.0; } - // 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"); + trajectoryZeroHoodMap = loadTrajectoriesV4("trajectories_0_hood.json"); } } else { if (trajectoryMap == null) { - trajectoryMap = loadTrajectories("trajectories.json"); + trajectoryMap = loadTrajectoriesV4("trajectories.json"); } } Rotation2d finalRotation = this.rotationToShootFrom(robotPose, targetPose); @@ -210,9 +236,7 @@ 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 = roundingDistance(distance); - this.aKitLog.record("V3DynamicOriginalDistanceInMeters", roundedDistance); var offsetDistance = roundedDistance + v3DistanceOffsetMeters.get(); - this.aKitLog.record("V3DynamicWithOffsetDistanceInMeters", offsetDistance); var key = new TrajectoryKey(roundingDistance(offsetDistance)); var hoodTrajectory = this.searchForHoodTrajectory(key, zeroHood); if (hoodTrajectory.isEmpty()) { @@ -232,24 +256,33 @@ private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targe return new ShootingData(finalRotation, Units.RPM.of(matchedTrajectory.RPM), matchedTrajectory.servo); } - private Optional searchForHoodTrajectory(TrajectoryKey key, boolean zeroHood) { - var mapToUse = zeroHood ? trajectoryZeroHoodMap : trajectoryMap; - - if (mapToUse.containsKey(key)) { - this.aKitLog.record("V3DynamicDeterminedDistanceInMeters", key.distance); - return Optional.of(mapToUse.get(key)); + // Look up optimal shooting parameters based on current pose and shooting + // target's pose. + private ShootingData calculateTrajectoryV4Dynamic(Pose2d robotPose, Pose2d targetPose, boolean zeroHood) { + if (trajectoryMap == null) { + trajectoryMap = loadTrajectoriesV4("trajectories.json"); + } + if (trajectoryZeroHoodMap == null) { + trajectoryZeroHoodMap = loadTrajectoriesV4("trajectories_0_hood.json"); } - var adjustedDistanceCheck = roundingDistance(key.distance + 0.01); - while (adjustedDistanceCheck < 10.0) { - var check = new TrajectoryKey(adjustedDistanceCheck); - if (mapToUse.containsKey(check)) { - this.aKitLog.record("V3DynamicDeterminedDistanceInMeters", check.distance); - return Optional.of(mapToUse.get(check)); - } - adjustedDistanceCheck = roundingDistance(adjustedDistanceCheck + 0.01); - log.info("adjustedDistanceCheck: {}", adjustedDistanceCheck); + 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(); + + HoodTrajectory result = activeMap.get(offsetDistance); + if (result == null) { + log.warn("Trajectory not found for distance: {}", offsetDistance); + return TrajectoriesCalculation.emptyShootingData; } + return new ShootingData(finalRotation, Units.RPM.of(result.RPM), result.servo); + } + + private Optional searchForHoodTrajectory(TrajectoryKey key, boolean zeroHood) { if (zeroHood) { log.error( @@ -283,6 +316,27 @@ private PresetShootingDistance getPresetShootingDistance(Distance distance) { // This method loads the trajectories from the JSON file and populates the // HashMap. + private InterpolatingTreeMap loadTrajectoriesV4(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) { + map.put(point.distance, point); + } + 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 map; // always returns map, even if empty + } + private HashMap loadTrajectories(String filename) { var loadedMap = new HashMap(); @@ -312,4 +366,6 @@ private HashMap loadTrajectories(String filename) return loadedMap; } -} + + +} \ 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 new file mode 100644 index 00000000..4d017c5c --- /dev/null +++ b/src/test/java/competition/subsystems/shooter/TrajectoriesCalculationTest.java @@ -0,0 +1,114 @@ +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 xbot.common.subsystems.pose.GameField; + +import java.util.Random; + +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); + + private TrajectoriesCalculation.ShootingData getData( + TrajectoriesCalculation calc, + Pose2d pose, + boolean zeroHood) { + + return calc.calculateAllianceHubShootingData(pose, zeroHood); + } + + @Test + public void testZeroHoodVsNormal() { + TrajectoriesCalculation calc = getInjectorComponent().trajectoriesCalculation(); + + + 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(); + + 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}) { + + var closeShot = getData(calc, closePose, zeroHood); + var midShot = getData(calc, midPose, zeroHood); + var farShot = getData(calc, farPose, zeroHood); + + assertNotNull(closeShot); + assertNotNull(midShot); + assertNotNull(farShot); + + 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(); + + long seed = System.currentTimeMillis(); + Random rand = new Random(seed); + + 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() * 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, 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.0 and 1.0", + data.servoRatio() <= 1.0); + } + } + } + } +} \ No newline at end of file