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..4551d9cd --- /dev/null +++ b/src/main/java/com/team1816/lib/util/BaseShooterCalculator.java @@ -0,0 +1,14 @@ +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 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 new file mode 100644 index 00000000..1806cb87 --- /dev/null +++ b/src/main/java/com/team1816/lib/util/IShooterCalculator.java @@ -0,0 +1,12 @@ +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 { + 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/ShooterTableCalculator.java b/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java index 0fcbf2d9..aebec0ab 100644 --- a/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java +++ b/src/main/java/com/team1816/lib/util/ShooterTableCalculator.java @@ -1,14 +1,21 @@ 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; +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 extends BaseShooterCalculator { - private final ShotLookup shotLookup; + private final PolynomialSplineFunction inclineAngleRotationsFunction, launchVelocityRPSFunction; public ShooterTableCalculator() { ShooterSettingsConfig shooterSettings = factory.getShooterSettingsConfig(); @@ -16,6 +23,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 +36,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..878c206b 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.IShooterCalculator; 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"); @@ -498,11 +501,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); } @@ -514,16 +514,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); + IShooterCalculator.ShooterCalculatorResponse response = shooterTableCalculator.getShooterSettings(getCurrentTurretPose2d().getTranslation(), + targetTranslation2d, useChassisSpeedForHoodAngleAndSpeed); + + setInclineAngle(response.inclineAngleRotations()); + setLaunchVelocities(response.launchVelocityRPS()); } /**