Skip to content

Linear offset for dynamic ranging. #411

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
rokadias wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Linear offset for dynamic ranging. #411

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
93 changes: 78 additions & 15 deletions elastic-layout/robot-configuration.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
},
{
Expand Down Expand Up @@ -68,8 +67,7 @@
"properties": {
"topic": "/SmartDashboard/Shoot from trench.",
"period": 0.06,
"show_type": false,
"maximize_button_space": false
"show_type": false
}
},
{
Expand Down Expand Up @@ -109,8 +107,7 @@
"properties": {
"topic": "/SmartDashboard/Shoot from hub.",
"period": 0.06,
"show_type": false,
"maximize_button_space": false
"show_type": false
}
},
{
Expand Down Expand Up @@ -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
}
},
{
Expand All @@ -169,8 +165,7 @@
"properties": {
"topic": "/SmartDashboard/Collect and shoot twice.",
"period": 0.06,
"show_type": true,
"maximize_button_space": false
"show_type": true
}
}
]
Expand Down Expand Up @@ -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",
Expand All @@ -1699,8 +1750,7 @@
"properties": {
"topic": "/SmartDashboard/AimAndShootFromHereCommand",
"period": 0.06,
"show_type": true,
"maximize_button_space": false
"show_type": true
}
},
{
Expand All @@ -1713,8 +1763,7 @@
"properties": {
"topic": "/SmartDashboard/DriveThroughAllianceTrenchCommand",
"period": 0.06,
"show_type": true,
"maximize_button_space": false
"show_type": true
}
},
{
Expand Down Expand Up @@ -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
}
}
]
}
Expand Down
34 changes: 33 additions & 1 deletion src/main/java/competition/subsystems/pose/TrajectoriesCalculation.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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);
}

Expand All @@ -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<TrajectoryKey, HoodTrajectory> trajectoryMap = null;
private static TrajectoryMapRange trajectoryMapRange = null;
private static HashMap<TrajectoryKey, HoodTrajectory> trajectoryZeroHoodMap = null;
private static TrajectoryMapRange trajectoryZeroHoodMapRange = null;

// CHECKSTYLE:OFF
// essentially holds all the values for the JSON to fill hashmap
Expand Down Expand Up @@ -191,17 +198,27 @@ 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) {
// 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");
trajectoryZeroHoodMapRange = buildTrajectoryRange(trajectoryZeroHoodMap);
}
} else {
if (trajectoryMap == null) {
trajectoryMap = loadTrajectories("trajectories.json");
trajectoryMapRange = buildTrajectoryRange(trajectoryMap);
}
}
Rotation2d finalRotation = this.rotationToShootFrom(robotPose, targetPose);
Expand All @@ -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);
Expand Down Expand Up @@ -281,6 +298,21 @@ private PresetShootingDistance getPresetShootingDistance(Distance distance) {
.orElse(PresetShootingDistance.NEAR);
}

private TrajectoryMapRange buildTrajectoryRange(HashMap<TrajectoryKey, HoodTrajectory> 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<TrajectoryKey, HoodTrajectory> loadTrajectories(String filename) {
Expand Down
Loading