Added abstract class for turret angle.

Author: shishghate

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);
+    }
+
+}

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 {

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;
+    }
 }

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) {

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) {