From 5b9149e6f28cadc4c7bff9e97254744fd31ffd12 Mon Sep 17 00:00:00 2001 From: Shishir Ghate Date: Thu, 26 Mar 2026 12:30:17 -0500 Subject: [PATCH 1/5] Put the linear calculator behind an interface for easier swapping. --- .../team1816/lib/util/IShooterCalculator.java | 7 ++++ .../lib/util/ShooterCalculatorResponse.java | 19 +++++++++ .../lib/util/ShooterTableCalculator.java | 41 ++++++++++++++---- .../com/team1816/lib/util/ShotLookup.java | 30 ------------- .../team1816/season/subsystems/Shooter.java | 42 +++++++------------ 5 files changed, 74 insertions(+), 65 deletions(-) create mode 100644 src/main/java/com/team1816/lib/util/IShooterCalculator.java create mode 100644 src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java delete mode 100644 src/main/java/com/team1816/lib/util/ShotLookup.java diff --git a/src/main/java/com/team1816/lib/util/IShooterCalculator.java b/src/main/java/com/team1816/lib/util/IShooterCalculator.java new file mode 100644 index 00000000..26f28a7c --- /dev/null +++ b/src/main/java/com/team1816/lib/util/IShooterCalculator.java @@ -0,0 +1,7 @@ +package com.team1816.lib.util; + +import edu.wpi.first.math.geometry.Translation2d; + +public interface IShooterCalculator { + ShooterCalculatorResponse getShooterSettings(Translation2d turret, Translation2d target, boolean useChassisSpeedForHoodAngleAndSpeed); +} diff --git a/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java b/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java new file mode 100644 index 00000000..565d1bdf --- /dev/null +++ b/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java @@ -0,0 +1,19 @@ +package com.team1816.lib.util; + +public class ShooterCalculatorResponse { + private double inclineAngleDegrees = 0; + private double launchVelocityRPS = 0; + + ShooterCalculatorResponse(double inclineAngleDegrees, double launchVelocityRPS) { + this.inclineAngleDegrees = inclineAngleDegrees; + this.launchVelocityRPS = launchVelocityRPS; + } + + public double getInclineAngelDegrees() { + return inclineAngleDegrees; + } + + public double getLaunchVelocityRPS() { + return launchVelocityRPS; + } +} diff --git a/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java b/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java index 0fcbf2d9..0b02a8c5 100644 --- a/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java +++ b/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java @@ -1,14 +1,19 @@ package com.team1816.lib.util; import com.team1816.lib.hardware.ShooterSettingsConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import org.apache.commons.math3.analysis.interpolation.LinearInterpolator; +import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; import java.util.List; import static com.team1816.lib.Singleton.factory; -public class ShooterTableCalculator { +public class ShooterTableCalculator implements IShooterCalculator { - private final ShotLookup shotLookup; + private final PolynomialSplineFunction inclineAngleRotationsFunction, launchVelocityRPSFunction; public ShooterTableCalculator() { ShooterSettingsConfig shooterSettings = factory.getShooterSettingsConfig(); @@ -16,6 +21,8 @@ public ShooterTableCalculator() { List distancesInches = shooterSettings.distancesInches; List inclineAnglesRotations = shooterSettings.inclineAnglesRotations; List launchVelocitiesRPS = shooterSettings.launchVelocitiesRPS; + LinearInterpolator inclineAngleRotationsLI = new LinearInterpolator(); + LinearInterpolator launchVelocityRPSLI = new LinearInterpolator(); double[] distancesInchesArray = distancesInches.stream() .mapToDouble(Double::doubleValue) @@ -27,15 +34,31 @@ public ShooterTableCalculator() { .mapToDouble(Double::doubleValue) .toArray(); - shotLookup = new ShotLookup(distancesInchesArray, inclineAnglesRotationsArray, launchVelocitiesRPSArray); + this.inclineAngleRotationsFunction = inclineAngleRotationsLI.interpolate(distancesInchesArray, inclineAnglesRotationsArray); + this.launchVelocityRPSFunction = launchVelocityRPSLI.interpolate(distancesInchesArray, launchVelocitiesRPSArray); } - public ShooterDistanceSetting getShooterDistanceSetting(double distanceInches) { - return new ShooterDistanceSetting( - shotLookup.getInclineAngleRotations(distanceInches), - shotLookup.getLaunchVelocityRPS(distanceInches) - ); + public ShooterCalculatorResponse getShooterSettings(Translation2d shooter, Translation2d target, boolean useChassisSpeedForHoodAngleAndSpeed) { + double distanceToTargetMeters = shooter.getDistance(target); + double distanceToTargetInches = Units.metersToInches(distanceToTargetMeters); + double inclineAngleRotations = getInclineAngleRotations(distanceToTargetInches); + double inclineAngleDegrees = Units.rotationsToDegrees(inclineAngleRotations); + double launchVelocityRPS = getLaunchVelocityRPS(distanceToTargetInches); + + return new ShooterCalculatorResponse(inclineAngleDegrees, launchVelocityRPS); + } + + private double getInclineAngleRotations(double distanceInches) { + var knots = inclineAngleRotationsFunction.getKnots(); + // Clamp the distance to within the interpolation range. + double clampedDistanceInches = MathUtil.clamp(distanceInches, knots[0], knots[knots.length - 1]); + return inclineAngleRotationsFunction.value(clampedDistanceInches); } - public record ShooterDistanceSetting(double inclineAngleRotations, double launchVelocityRPS) {} + private double getLaunchVelocityRPS(double distanceInches) { + var knots = launchVelocityRPSFunction.getKnots(); + // Clamp the distance to within the interpolation range. + double clampedDistanceInches = MathUtil.clamp(distanceInches, knots[0], knots[knots.length - 1]); + return launchVelocityRPSFunction.value(clampedDistanceInches); + } } diff --git a/src/main/java/com/team1816/lib/util/ShotLookup.java b/src/main/java/com/team1816/lib/util/ShotLookup.java deleted file mode 100644 index 8e15fe61..00000000 --- a/src/main/java/com/team1816/lib/util/ShotLookup.java +++ /dev/null @@ -1,30 +0,0 @@ -package com.team1816.lib.util; - -import edu.wpi.first.math.MathUtil; -import org.apache.commons.math3.analysis.interpolation.LinearInterpolator; -import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; - -public class ShotLookup { - private final PolynomialSplineFunction inclineAngleRotationsFunction, launchVelocityRPSFunction; - - public ShotLookup(double[] distancesInches, double[] inclineAnglesRotations, double[] launchVelocitiesRPS) { - LinearInterpolator inclineAngleRotationsLI = new LinearInterpolator(); - LinearInterpolator launchVelocityRPSLI = new LinearInterpolator(); - this.inclineAngleRotationsFunction = inclineAngleRotationsLI.interpolate(distancesInches, inclineAnglesRotations); - this.launchVelocityRPSFunction = launchVelocityRPSLI.interpolate(distancesInches, launchVelocitiesRPS); - } - - public double getInclineAngleRotations(double distanceInches) { - var knots = inclineAngleRotationsFunction.getKnots(); - // Clamp the distance to within the interpolation range. - double clampedDistanceInches = MathUtil.clamp(distanceInches, knots[0], knots[knots.length - 1]); - return inclineAngleRotationsFunction.value(clampedDistanceInches); - } - - public double getLaunchVelocityRPS(double distanceInches) { - var knots = launchVelocityRPSFunction.getKnots(); - // Clamp the distance to within the interpolation range. - double clampedDistanceInches = MathUtil.clamp(distanceInches, knots[0], knots[knots.length - 1]); - return launchVelocityRPSFunction.value(clampedDistanceInches); - } -} diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index ff5dd82f..f4d4e49a 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -11,6 +11,7 @@ import com.team1816.lib.subsystems.ITestableSubsystem; import com.team1816.lib.util.FieldContainer; import com.team1816.lib.util.GreenLogger; +import com.team1816.lib.util.ShooterCalculatorResponse; import com.team1816.lib.util.ShooterTableCalculator; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.*; @@ -84,6 +85,8 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { */ private double turretAngleAdjustmentDegrees = 0; + private boolean useChassisSpeedForHoodAngleAndSpeed = false; + //MOTORS private final IMotor topLaunchMotor = (IMotor) factory.getDevice(NAME, "topLaunchMotor"); private final IMotor bottomLaunchMotor = (IMotor) factory.getDevice(NAME, "bottomLaunchMotor"); @@ -316,27 +319,19 @@ public void readFromHardware() { private void applyState() { Translation2d target = Translation2d.kZero; + if (autoAimTurret || wantedDistanceState == ShooterDistanceState.AUTOMATIC) { - isAutoAiming = true; target = getTargetTranslation2d(); - } - else { - isAutoAiming = false; - } - - if (autoAimTurret) { aimTurretAtTarget(target); - } - else { + aimInclineAndLaunchersAtTarget(target); + } else { setTurretAngle(turretFixedAngleDegrees); - } - - switch (wantedDistanceState) { - case IDLE, PRESET_CLOSE, PRESET_MIDDLE, PRESET_FAR, PRESET_AUTO_THING -> { - setInclineAngle(wantedDistanceState.getInclineAngleDegrees()); - setLaunchVelocities(wantedDistanceState.getLaunchVelocityRPS()); + switch (wantedDistanceState) { + case IDLE, PRESET_CLOSE, PRESET_MIDDLE, PRESET_FAR, PRESET_AUTO_THING -> { + setInclineAngle(wantedDistanceState.getInclineAngleDegrees()); + setLaunchVelocities(wantedDistanceState.getLaunchVelocityRPS()); + } } - case AUTOMATIC -> aimInclineAndLaunchersAtTarget(target); } } @@ -514,16 +509,11 @@ private void aimTurretAtTarget(Translation2d targetTranslation2d) { * @param targetTranslation2d The {@link Translation2d} of the target to aim at. */ private void aimInclineAndLaunchersAtTarget(Translation2d targetTranslation2d) { - Translation2d shooterTranslation2d = getCurrentTurretPose2d().getTranslation(); - double distanceToTargetMeters = shooterTranslation2d.getDistance(targetTranslation2d); - double distanceToTargetInches = Units.metersToInches(distanceToTargetMeters); - ShooterTableCalculator.ShooterDistanceSetting shooterDistanceSetting = shooterTableCalculator - .getShooterDistanceSetting(distanceToTargetInches); - double inclineAngleRotations = shooterDistanceSetting.inclineAngleRotations(); - double inclineAngleDegrees = Units.rotationsToDegrees(inclineAngleRotations); - double launchVelocityRPS = shooterDistanceSetting.launchVelocityRPS(); - setInclineAngle(inclineAngleDegrees); - setLaunchVelocities(launchVelocityRPS); + ShooterCalculatorResponse response = shooterTableCalculator.getShooterSettings(getCurrentTurretPose2d().getTranslation(), + targetTranslation2d, useChassisSpeedForHoodAngleAndSpeed); + + setInclineAngle(response.getInclineAngelDegrees()); + setLaunchVelocities(response.getLaunchVelocityRPS()); } /** From 11005acfc26935330030aaba657e2e69659ec8a3 Mon Sep 17 00:00:00 2001 From: Shishir Ghate Date: Thu, 26 Mar 2026 17:11:21 -0500 Subject: [PATCH 2/5] Added abstract class for turret angle. --- .../team1816/lib/util/BaseShooterCalculator.java | 15 +++++++++++++++ .../com/team1816/lib/util/IShooterCalculator.java | 2 ++ .../lib/util/ShooterCalculatorResponse.java | 10 +++++++++- .../team1816/lib/util/ShooterTableCalculator.java | 6 ++++-- .../com/team1816/season/subsystems/Shooter.java | 10 ++++++++-- 5 files changed, 38 insertions(+), 5 deletions(-) create mode 100644 src/main/java/com/team1816/lib/util/BaseShooterCalculator.java diff --git a/src/main/java/com/team1816/lib/util/BaseShooterCalculator.java b/src/main/java/com/team1816/lib/util/BaseShooterCalculator.java new file mode 100644 index 00000000..38c568f0 --- /dev/null +++ b/src/main/java/com/team1816/lib/util/BaseShooterCalculator.java @@ -0,0 +1,15 @@ +package com.team1816.lib.util; + +import com.team1816.lib.BaseRobotState; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public abstract class BaseShooterCalculator implements IShooterCalculator { + public Rotation2d getTurrentAngle(Translation2d shooter, Translation2d target) { + Translation2d shooterToTargetTranslation2d = target.minus(shooter); + Rotation2d fieldRelativeRotation2dToTarget = shooterToTargetTranslation2d.getAngle(); + Rotation2d robotRotation2d = BaseRobotState.robotPose.getRotation(); + return fieldRelativeRotation2dToTarget.minus(robotRotation2d); + } + +} diff --git a/src/main/java/com/team1816/lib/util/IShooterCalculator.java b/src/main/java/com/team1816/lib/util/IShooterCalculator.java index 26f28a7c..784f2d93 100644 --- a/src/main/java/com/team1816/lib/util/IShooterCalculator.java +++ b/src/main/java/com/team1816/lib/util/IShooterCalculator.java @@ -1,5 +1,7 @@ package com.team1816.lib.util; +import com.team1816.lib.BaseRobotState; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; public interface IShooterCalculator { diff --git a/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java b/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java index 565d1bdf..2020a295 100644 --- a/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java +++ b/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java @@ -1,10 +1,14 @@ package com.team1816.lib.util; +import edu.wpi.first.math.geometry.Rotation2d; + public class ShooterCalculatorResponse { private double inclineAngleDegrees = 0; private double launchVelocityRPS = 0; + private Rotation2d turretAngle = null; - ShooterCalculatorResponse(double inclineAngleDegrees, double launchVelocityRPS) { + ShooterCalculatorResponse(Rotation2d turretAngle, double inclineAngleDegrees, double launchVelocityRPS) { + this.turretAngle = turretAngle; this.inclineAngleDegrees = inclineAngleDegrees; this.launchVelocityRPS = launchVelocityRPS; } @@ -16,4 +20,8 @@ public double getInclineAngelDegrees() { public double getLaunchVelocityRPS() { return launchVelocityRPS; } + + public Rotation2d getTurrentAngle() { + return turretAngle; + } } diff --git a/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java b/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java index 0b02a8c5..4145c682 100644 --- a/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java +++ b/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java @@ -1,7 +1,9 @@ package com.team1816.lib.util; +import com.team1816.lib.BaseRobotState; import com.team1816.lib.hardware.ShooterSettingsConfig; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import org.apache.commons.math3.analysis.interpolation.LinearInterpolator; @@ -11,7 +13,7 @@ import static com.team1816.lib.Singleton.factory; -public class ShooterTableCalculator implements IShooterCalculator { +public class ShooterTableCalculator extends BaseShooterCalculator { private final PolynomialSplineFunction inclineAngleRotationsFunction, launchVelocityRPSFunction; @@ -45,7 +47,7 @@ public ShooterCalculatorResponse getShooterSettings(Translation2d shooter, Trans double inclineAngleDegrees = Units.rotationsToDegrees(inclineAngleRotations); double launchVelocityRPS = getLaunchVelocityRPS(distanceToTargetInches); - return new ShooterCalculatorResponse(inclineAngleDegrees, launchVelocityRPS); + return new ShooterCalculatorResponse(getTurrentAngle(shooter, target), inclineAngleDegrees, launchVelocityRPS); } private double getInclineAngleRotations(double distanceInches) { diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index f4d4e49a..2e9a6dd4 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -322,8 +322,14 @@ private void applyState() { if (autoAimTurret || wantedDistanceState == ShooterDistanceState.AUTOMATIC) { target = getTargetTranslation2d(); - aimTurretAtTarget(target); - aimInclineAndLaunchersAtTarget(target); + ShooterCalculatorResponse response = shooterTableCalculator.getShooterSettings(getCurrentTurretPose2d().getTranslation(), + target, useChassisSpeedForHoodAngleAndSpeed); + + double robotRelativeDegreesToTarget = response.getTurrentAngle().getDegrees(); + + setTurretAngle(robotRelativeDegreesToTarget); + setInclineAngle(response.getInclineAngelDegrees()); + setLaunchVelocities(response.getLaunchVelocityRPS()); } else { setTurretAngle(turretFixedAngleDegrees); switch (wantedDistanceState) { From 8182e584d54c8a4ec11de7b04f10fbc2b2a8de3a Mon Sep 17 00:00:00 2001 From: Shishir Ghate Date: Thu, 26 Mar 2026 17:31:46 -0500 Subject: [PATCH 3/5] Reset to old applyState logic and make adjustments --- .../lib/util/BaseShooterCalculator.java | 3 +- .../team1816/lib/util/IShooterCalculator.java | 1 + .../lib/util/ShooterCalculatorResponse.java | 8 +---- .../lib/util/ShooterTableCalculator.java | 2 +- .../team1816/season/subsystems/Shooter.java | 32 ++++++++++++++++--- 5 files changed, 31 insertions(+), 15 deletions(-) diff --git a/src/main/java/com/team1816/lib/util/BaseShooterCalculator.java b/src/main/java/com/team1816/lib/util/BaseShooterCalculator.java index 38c568f0..4551d9cd 100644 --- a/src/main/java/com/team1816/lib/util/BaseShooterCalculator.java +++ b/src/main/java/com/team1816/lib/util/BaseShooterCalculator.java @@ -5,11 +5,10 @@ import edu.wpi.first.math.geometry.Translation2d; public abstract class BaseShooterCalculator implements IShooterCalculator { - public Rotation2d getTurrentAngle(Translation2d shooter, Translation2d target) { + public Rotation2d getTurretAngle(Translation2d shooter, Translation2d target, boolean useChassisSpeedForHoodAngleAndSpeed) { Translation2d shooterToTargetTranslation2d = target.minus(shooter); Rotation2d fieldRelativeRotation2dToTarget = shooterToTargetTranslation2d.getAngle(); Rotation2d robotRotation2d = BaseRobotState.robotPose.getRotation(); return fieldRelativeRotation2dToTarget.minus(robotRotation2d); } - } diff --git a/src/main/java/com/team1816/lib/util/IShooterCalculator.java b/src/main/java/com/team1816/lib/util/IShooterCalculator.java index 784f2d93..7c48f435 100644 --- a/src/main/java/com/team1816/lib/util/IShooterCalculator.java +++ b/src/main/java/com/team1816/lib/util/IShooterCalculator.java @@ -5,5 +5,6 @@ import edu.wpi.first.math.geometry.Translation2d; public interface IShooterCalculator { + Rotation2d getTurretAngle(Translation2d turret, Translation2d target, boolean useChassisSpeedForHoodAngleAndSpeed); ShooterCalculatorResponse getShooterSettings(Translation2d turret, Translation2d target, boolean useChassisSpeedForHoodAngleAndSpeed); } diff --git a/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java b/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java index 2020a295..329fde1e 100644 --- a/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java +++ b/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java @@ -5,10 +5,8 @@ public class ShooterCalculatorResponse { private double inclineAngleDegrees = 0; private double launchVelocityRPS = 0; - private Rotation2d turretAngle = null; - ShooterCalculatorResponse(Rotation2d turretAngle, double inclineAngleDegrees, double launchVelocityRPS) { - this.turretAngle = turretAngle; + ShooterCalculatorResponse(double inclineAngleDegrees, double launchVelocityRPS) { this.inclineAngleDegrees = inclineAngleDegrees; this.launchVelocityRPS = launchVelocityRPS; } @@ -20,8 +18,4 @@ public double getInclineAngelDegrees() { public double getLaunchVelocityRPS() { return launchVelocityRPS; } - - public Rotation2d getTurrentAngle() { - return turretAngle; - } } diff --git a/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java b/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java index 4145c682..aebec0ab 100644 --- a/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java +++ b/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java @@ -47,7 +47,7 @@ public ShooterCalculatorResponse getShooterSettings(Translation2d shooter, Trans double inclineAngleDegrees = Units.rotationsToDegrees(inclineAngleRotations); double launchVelocityRPS = getLaunchVelocityRPS(distanceToTargetInches); - return new ShooterCalculatorResponse(getTurrentAngle(shooter, target), inclineAngleDegrees, launchVelocityRPS); + return new ShooterCalculatorResponse(inclineAngleDegrees, launchVelocityRPS); } private double getInclineAngleRotations(double distanceInches) { diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 2e9a6dd4..3c37fb87 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -319,6 +319,30 @@ public void readFromHardware() { private void applyState() { Translation2d target = Translation2d.kZero; + if (autoAimTurret || wantedDistanceState == ShooterDistanceState.AUTOMATIC) { + isAutoAiming = true; + target = getTargetTranslation2d(); + } + else { + isAutoAiming = false; + } + + if (autoAimTurret) { + aimTurretAtTarget(target); + } + else { + setTurretAngle(turretFixedAngleDegrees); + } + + switch (wantedDistanceState) { + case IDLE, PRESET_CLOSE, PRESET_MIDDLE, PRESET_FAR, PRESET_AUTO_THING -> { + setInclineAngle(wantedDistanceState.getInclineAngleDegrees()); + setLaunchVelocities(wantedDistanceState.getLaunchVelocityRPS()); + } + case AUTOMATIC -> aimInclineAndLaunchersAtTarget(target); + } + +/* Translation2d target = Translation2d.kZero; if (autoAimTurret || wantedDistanceState == ShooterDistanceState.AUTOMATIC) { target = getTargetTranslation2d(); @@ -339,6 +363,7 @@ private void applyState() { } } } + */ } public void setWantedDistanceState(ShooterDistanceState state) { @@ -499,11 +524,8 @@ private Translation2d flipTranslation2dBasedOnAlliance(Translation2d blueTransla * @param targetTranslation2d The {@link Translation2d} of the target to aim at. */ private void aimTurretAtTarget(Translation2d targetTranslation2d) { - Translation2d shooterTranslation2d = getCurrentTurretPose2d().getTranslation(); - Translation2d shooterToTargetTranslation2d = targetTranslation2d.minus(shooterTranslation2d); - Rotation2d fieldRelativeRotation2dToTarget = shooterToTargetTranslation2d.getAngle(); - Rotation2d robotRotation2d = BaseRobotState.robotPose.getRotation(); - Rotation2d robotRelativeRotation2dToTarget = fieldRelativeRotation2dToTarget.minus(robotRotation2d); + Rotation2d robotRelativeRotation2dToTarget = shooterTableCalculator.getTurretAngle(getCurrentTurretPose2d().getTranslation(), + targetTranslation2d, useChassisSpeedForHoodAngleAndSpeed); double robotRelativeDegreesToTarget = robotRelativeRotation2dToTarget.getDegrees(); setTurretAngle(robotRelativeDegreesToTarget); } From 9d8c7a09819bf2f453183884b4be3a08b361606e Mon Sep 17 00:00:00 2001 From: Shishir Ghate Date: Thu, 26 Mar 2026 17:42:35 -0500 Subject: [PATCH 4/5] Convert back to record class for response. --- .../team1816/lib/util/IShooterCalculator.java | 2 ++ .../lib/util/ShooterCalculatorResponse.java | 21 ------------------- .../team1816/season/subsystems/Shooter.java | 8 +++---- 3 files changed, 6 insertions(+), 25 deletions(-) delete mode 100644 src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java diff --git a/src/main/java/com/team1816/lib/util/IShooterCalculator.java b/src/main/java/com/team1816/lib/util/IShooterCalculator.java index 7c48f435..1806cb87 100644 --- a/src/main/java/com/team1816/lib/util/IShooterCalculator.java +++ b/src/main/java/com/team1816/lib/util/IShooterCalculator.java @@ -7,4 +7,6 @@ public interface IShooterCalculator { Rotation2d getTurretAngle(Translation2d turret, Translation2d target, boolean useChassisSpeedForHoodAngleAndSpeed); ShooterCalculatorResponse getShooterSettings(Translation2d turret, Translation2d target, boolean useChassisSpeedForHoodAngleAndSpeed); + + record ShooterCalculatorResponse(double inclineAngleRotations, double launchVelocityRPS) {} } diff --git a/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java b/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java deleted file mode 100644 index 329fde1e..00000000 --- a/src/main/java/com/team1816/lib/util/ShooterCalculatorResponse.java +++ /dev/null @@ -1,21 +0,0 @@ -package com.team1816.lib.util; - -import edu.wpi.first.math.geometry.Rotation2d; - -public class ShooterCalculatorResponse { - private double inclineAngleDegrees = 0; - private double launchVelocityRPS = 0; - - ShooterCalculatorResponse(double inclineAngleDegrees, double launchVelocityRPS) { - this.inclineAngleDegrees = inclineAngleDegrees; - this.launchVelocityRPS = launchVelocityRPS; - } - - public double getInclineAngelDegrees() { - return inclineAngleDegrees; - } - - public double getLaunchVelocityRPS() { - return launchVelocityRPS; - } -} diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 3c37fb87..aa7f9368 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -11,7 +11,7 @@ import com.team1816.lib.subsystems.ITestableSubsystem; import com.team1816.lib.util.FieldContainer; import com.team1816.lib.util.GreenLogger; -import com.team1816.lib.util.ShooterCalculatorResponse; +import com.team1816.lib.util.IShooterCalculator; import com.team1816.lib.util.ShooterTableCalculator; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.*; @@ -537,11 +537,11 @@ private void aimTurretAtTarget(Translation2d targetTranslation2d) { * @param targetTranslation2d The {@link Translation2d} of the target to aim at. */ private void aimInclineAndLaunchersAtTarget(Translation2d targetTranslation2d) { - ShooterCalculatorResponse response = shooterTableCalculator.getShooterSettings(getCurrentTurretPose2d().getTranslation(), + IShooterCalculator.ShooterCalculatorResponse response = shooterTableCalculator.getShooterSettings(getCurrentTurretPose2d().getTranslation(), targetTranslation2d, useChassisSpeedForHoodAngleAndSpeed); - setInclineAngle(response.getInclineAngelDegrees()); - setLaunchVelocities(response.getLaunchVelocityRPS()); + setInclineAngle(response.inclineAngleRotations()); + setLaunchVelocities(response.launchVelocityRPS()); } /** From bc5564451b5b6dab3aa92d4fa78134712c7a7f64 Mon Sep 17 00:00:00 2001 From: Shishir Ghate Date: Thu, 26 Mar 2026 17:55:05 -0500 Subject: [PATCH 5/5] Removed commented out code --- .../team1816/season/subsystems/Shooter.java | 23 ------------------- 1 file changed, 23 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index aa7f9368..878c206b 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -341,29 +341,6 @@ private void applyState() { } case AUTOMATIC -> aimInclineAndLaunchersAtTarget(target); } - -/* Translation2d target = Translation2d.kZero; - - if (autoAimTurret || wantedDistanceState == ShooterDistanceState.AUTOMATIC) { - target = getTargetTranslation2d(); - ShooterCalculatorResponse response = shooterTableCalculator.getShooterSettings(getCurrentTurretPose2d().getTranslation(), - target, useChassisSpeedForHoodAngleAndSpeed); - - double robotRelativeDegreesToTarget = response.getTurrentAngle().getDegrees(); - - setTurretAngle(robotRelativeDegreesToTarget); - setInclineAngle(response.getInclineAngelDegrees()); - setLaunchVelocities(response.getLaunchVelocityRPS()); - } else { - setTurretAngle(turretFixedAngleDegrees); - switch (wantedDistanceState) { - case IDLE, PRESET_CLOSE, PRESET_MIDDLE, PRESET_FAR, PRESET_AUTO_THING -> { - setInclineAngle(wantedDistanceState.getInclineAngleDegrees()); - setLaunchVelocities(wantedDistanceState.getLaunchVelocityRPS()); - } - } - } - */ } public void setWantedDistanceState(ShooterDistanceState state) {