From 9f63c95e16fa94e693183b15eae3c1b597d4fb91 Mon Sep 17 00:00:00 2001 From: Saqib Rokadia Date: Fri, 10 Apr 2026 09:01:05 -0700 Subject: [PATCH] Using CalcV3 with dynamic ranging. --- elastic-layout/robot-configuration.json | 93 ++++++++++++++++--- .../pose/TrajectoriesCalculation.java | 34 ++++++- 2 files changed, 111 insertions(+), 16 deletions(-) diff --git a/elastic-layout/robot-configuration.json b/elastic-layout/robot-configuration.json index e990a5c5..a9009ac3 100644 --- a/elastic-layout/robot-configuration.json +++ b/elastic-layout/robot-configuration.json @@ -27,8 +27,7 @@ "properties": { "topic": "/SmartDashboard/Shoot from trench then collect from netural zone.", "period": 0.06, - "show_type": false, - "maximize_button_space": false + "show_type": false } }, { @@ -68,8 +67,7 @@ "properties": { "topic": "/SmartDashboard/Shoot from trench.", "period": 0.06, - "show_type": false, - "maximize_button_space": false + "show_type": false } }, { @@ -109,8 +107,7 @@ "properties": { "topic": "/SmartDashboard/Shoot from hub.", "period": 0.06, - "show_type": false, - "maximize_button_space": false + "show_type": false } }, { @@ -141,8 +138,7 @@ "properties": { "topic": "/SmartDashboard/Move midway through field and back.", "period": 0.06, - "show_type": false, - "maximize_button_space": false + "show_type": false } }, { @@ -169,8 +165,7 @@ "properties": { "topic": "/SmartDashboard/Collect and shoot twice.", "period": 0.06, - "show_type": true, - "maximize_button_space": false + "show_type": true } } ] @@ -1687,7 +1682,63 @@ { "name": "Test Labs for Auto!", "grid_layout": { - "layouts": [], + "layouts": [ + { + "title": "Calculated Values", + "x": 512.0, + "y": 128.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Distance Determined by Pose", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/TrajectoriesCalculationV3DynamicOriginalDistanceInMeters", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Distance With Offset", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/TrajectoriesCalculationV3DynamicWithOffsetDistanceInMeters", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Distance Found in Trajectories Json", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/TrajectoriesCalculationV3DynamicDeterminedDistanceInMeters", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + ], "containers": [ { "title": "AimAndShootFromHereCommand", @@ -1699,8 +1750,7 @@ "properties": { "topic": "/SmartDashboard/AimAndShootFromHereCommand", "period": 0.06, - "show_type": true, - "maximize_button_space": false + "show_type": true } }, { @@ -1713,8 +1763,7 @@ "properties": { "topic": "/SmartDashboard/DriveThroughAllianceTrenchCommand", "period": 0.06, - "show_type": true, - "maximize_button_space": false + "show_type": true } }, { @@ -1744,6 +1793,20 @@ "data_type": "double", "show_submit_button": true } + }, + { + "title": "v3DistanceOffsetSlope", + "x": 128.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/TrajectoriesCalculation/v3DistanceOffsetSlope", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } } ] } diff --git a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java index 519a41d7..bd2d2b5a 100644 --- a/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java +++ b/src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java @@ -43,6 +43,7 @@ public class TrajectoriesCalculation { private final DoubleProperty trajectoriesShooterRPMFixed; private final DoubleProperty interpolationFactor; private final DoubleProperty v3DistanceOffsetMeters; + private final DoubleProperty v3DistanceOffsetSlope; private final StringProperty trajectoryCalcVersion; private AKitLogger aKitLog; @@ -83,6 +84,7 @@ public TrajectoriesCalculation(AprilTagFieldLayout aprilTagFieldLayout, Property 0.5); this.trajectoryCalcVersion = propManager.createPersistentProperty("TrajectoryCalcVersion", "3"); this.v3DistanceOffsetMeters = propManager.createPersistentProperty("v3DistanceOffsetMeters", 0.45); + this.v3DistanceOffsetSlope = propManager.createPersistentProperty("v3DistanceOffsetSlope", 0); getOrCreatePresetLookup(propManager); } @@ -102,9 +104,14 @@ private record PresetShootingProperties(DoubleProperty shooterRpmProperty, Doubl private record TrajectoryKey(double distance) { } + private record TrajectoryMapRange(double minimum, double maximum, double delta) { + } + // hashmaps holding all the values private static HashMap trajectoryMap = null; + private static TrajectoryMapRange trajectoryMapRange = null; private static HashMap trajectoryZeroHoodMap = null; + private static TrajectoryMapRange trajectoryZeroHoodMapRange = null; // CHECKSTYLE:OFF // essentially holds all the values for the JSON to fill hashmap @@ -191,6 +198,14 @@ private double roundingDistance(double distance) { return Math.round(distance * 100.0) / 100.0; } + private double calculateOffset(double distance, boolean zeroHood) { + var rangeToUse = zeroHood ? trajectoryZeroHoodMapRange : trajectoryMapRange; + + var deltaFromMinimum = distance - rangeToUse.minimum; + var offset = (v3DistanceOffsetSlope.get() * (deltaFromMinimum / rangeToUse.delta)) + v3DistanceOffsetMeters.get(); + return roundingDistance(offset); + } + // Look up optimal shooting parameters based on current pose and shooting // target's pose. private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targetPose, boolean zeroHood) { @@ -198,10 +213,12 @@ private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targe if (zeroHood) { if (trajectoryZeroHoodMap == null) { trajectoryZeroHoodMap = loadTrajectories("trajectories_0_hood.json"); + trajectoryZeroHoodMapRange = buildTrajectoryRange(trajectoryZeroHoodMap); } } else { if (trajectoryMap == null) { trajectoryMap = loadTrajectories("trajectories.json"); + trajectoryMapRange = buildTrajectoryRange(trajectoryMap); } } Rotation2d finalRotation = this.rotationToShootFrom(robotPose, targetPose); @@ -211,7 +228,7 @@ private ShootingData calculateTrajectoryV3Dynamic(Pose2d robotPose, Pose2d targe double distance = shooterPose.getTranslation().getDistance(targetPose.getTranslation()); var roundedDistance = roundingDistance(distance); this.aKitLog.record("V3DynamicOriginalDistanceInMeters", roundedDistance); - var offsetDistance = roundedDistance + v3DistanceOffsetMeters.get(); + var offsetDistance = roundingDistance(calculateOffset(roundedDistance, zeroHood) + roundedDistance); this.aKitLog.record("V3DynamicWithOffsetDistanceInMeters", offsetDistance); var key = new TrajectoryKey(roundingDistance(offsetDistance)); var hoodTrajectory = this.searchForHoodTrajectory(key, zeroHood); @@ -281,6 +298,21 @@ private PresetShootingDistance getPresetShootingDistance(Distance distance) { .orElse(PresetShootingDistance.NEAR); } + private TrajectoryMapRange buildTrajectoryRange(HashMap map) { + var minimum = 0.0; + var maximum = 0.0; + for (TrajectoryKey key : map.keySet()) { + if (minimum == 0.0 || key.distance < minimum) { + minimum = key.distance; + } + if (maximum == 0.0 || key.distance > maximum) { + maximum = key.distance; + } + } + + return new TrajectoryMapRange(minimum, maximum, maximum - minimum); + } + // This method loads the trajectories from the JSON file and populates the // HashMap. private HashMap loadTrajectories(String filename) {