added shooting while moving? refactor shooter distance getters

grt192 · Apr 4, 2024 · 52ce7f0 · 52ce7f0
1 parent 297294b
commit 52ce7f0
Show file tree

Hide file tree

Showing 6 changed files with 120 additions and 105 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -158,11 +158,11 @@ public RobotContainer() {
         intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem);
 
         shooterPivotSubsystem = new ShooterPivotSubsystem(
-            swerveSubsystem::getRobotPosition, 
+            swerveSubsystem::getShootingDistance, 
             fmsSubsystem::isRedAlliance
         );
         shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(
-            swerveSubsystem::getRobotPosition, 
+            swerveSubsystem::getShootingDistance, 
             fmsSubsystem::isRedAlliance
         );
 

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
@@ -23,6 +23,7 @@
 import frc.robot.util.Pose2dSupplier;
 
 import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
 
 import org.apache.commons.math3.analysis.interpolation.AkimaSplineInterpolator;
 import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction;
@@ -45,15 +46,14 @@ public class ShooterFlywheelSubsystem extends SubsystemBase {
     private double targetTopRPS;
     private double targetBottomRPS;
 
-    private Pose2dSupplier poseSupplier;
+    private DoubleSupplier distanceSupplier;
     private BooleanSupplier redSupplier;
 
     private AkimaSplineInterpolator akima;
     private PolynomialSplineFunction topFlywheelSpline;
     private PolynomialSplineFunction bottomFlywheelSpline;
     private boolean atSpeed = false;
     private boolean autoAim = false;
-    private Pose2d targetPose;
 
     private NetworkTable motorsNTTable;
     private NetworkTableEntry shooter13CurrentEntry;
@@ -67,7 +67,7 @@ public class ShooterFlywheelSubsystem extends SubsystemBase {
      *
      * @param poseSupplier The poseSupplier for shooter speed calculations.
      */
-    public ShooterFlywheelSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier redSupplier) {
+    public ShooterFlywheelSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier redSupplier) {
         //motors
         shooterMotorTop = new TalonFX(ShooterConstants.SHOOTER_MOTOR_TOP_ID);
         shooterMotorBottom = new TalonFX(ShooterConstants.SHOOTER_MOTOR_BOTTOM_ID);
@@ -107,7 +107,7 @@ public ShooterFlywheelSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier red
         shooterMotorBottom.getConfigurator().apply(configs);
         shooterMotorTop.getConfigurator().apply(configs);
 
-        this.poseSupplier = poseSupplier;
+        this.distanceSupplier = distanceSupplier;
         this.redSupplier = redSupplier;
         //nts
         ntInstance = NetworkTableInstance.getDefault();
@@ -226,24 +226,7 @@ public double getTargetBottomRPS() {
     }
 
     public double getShootingDistance() {
-        double currentDistance;
-        Pose2d currentField = poseSupplier.getPose2d();
-
-        if (redSupplier.getAsBoolean()) {  //true = red
-            double xLength = Math.pow(currentField.getX() - SwerveConstants.RED_SPEAKER_POS.getX(), 2);
-            double yLength = Math.pow(currentField.getY() - SwerveConstants.RED_SPEAKER_POS.getY(), 2);
-
-            currentDistance = Math.sqrt(xLength + yLength);
-        } else {
-            double xLength = Math.pow(currentField.getX() - SwerveConstants.BLUE_SPEAKER_POS.getX(), 2);
-            double yLength = Math.pow(currentField.getY() - SwerveConstants.BLUE_SPEAKER_POS.getY(), 2);
-
-            currentDistance = Math.sqrt(xLength + yLength);
-        }
-
-        return MathUtil.clamp(currentDistance,
-            ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE
-        );
+        return distanceSupplier.getAsDouble();
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -22,6 +22,7 @@
 import frc.robot.util.Pose2dSupplier;
 
 import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
 
 import org.apache.commons.math3.analysis.interpolation.AkimaSplineInterpolator;
 import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction;
@@ -47,7 +48,7 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     private double currentEncoderAngle;
     private double currentDistance;
 
-    private Pose2dSupplier poseSupplier; //new Pose2d();
+    private DoubleSupplier distanceSupplier; //new Pose2d();
     private BooleanSupplier redSupplier;
 
     private AkimaSplineInterpolator akima;
@@ -64,10 +65,10 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     private NetworkTableEntry shooter12TemperatureEntry;
 
     /** Inits motors and pose field. Also inits PID stuff. */
-    public ShooterPivotSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier redSupplier) {
+    public ShooterPivotSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier redSupplier) {
 
         timer.start();
-        this.poseSupplier = poseSupplier;
+        this.distanceSupplier = distanceSupplier;
         this.redSupplier = redSupplier;
 
         //motors
@@ -147,23 +148,7 @@ public void setAngle(double angle) {
     }
 
     private double getShootingDistance() {
-        Pose2d currentField = poseSupplier.getPose2d();
-
-        if (redSupplier.getAsBoolean()) {  //true = red
-            double xLength = Math.pow(currentField.getX() - SwerveConstants.RED_SPEAKER_POS.getX(), 2);
-            double yLength = Math.pow(currentField.getY() - SwerveConstants.RED_SPEAKER_POS.getY(), 2);
-
-            currentDistance = Math.sqrt(xLength + yLength);
-        } else {
-            double xLength = Math.pow(currentField.getX() - SwerveConstants.BLUE_SPEAKER_POS.getX(), 2);
-            double yLength = Math.pow(currentField.getY() - SwerveConstants.BLUE_SPEAKER_POS.getY(), 2);
-
-            currentDistance = Math.sqrt(xLength + yLength);
-        }
-
-        return MathUtil.clamp(currentDistance, 
-                              ShooterConstants.MIN_SHOOTER_DISTANCE, 
-                              ShooterConstants.MAX_SHOOTER_DISTANCE);
+        return distanceSupplier.getAsDouble();
     }
 
     /** Gets correct Angle for pivot to turn to. */

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveField.java b/src/main/java/frc/robot/subsystems/swerve/SwerveField.java
@@ -0,0 +1,10 @@
+package frc.robot.subsystems.swerve;
+
+public class SwerveField {
+
+    private SwerveSubsystem swerveSubsystem;
+
+    public SwerveField(SwerveSubsystem swerveSubsystem) {
+        this.swerveSubsystem = swerveSubsystem;
+    }
+}