fixed shooting positions.

grt192 · Apr 2, 2024 · 2df9312 · 2df9312
1 parent c66855a
commit 2df9312
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 21 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -83,9 +83,9 @@ public static class SwerveConstants {
         public static final Translation2d BL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST);
         public static final Translation2d BR_POS = new Translation2d(-MODULE_DIST, -MODULE_DIST);
 
-        public static final Translation2d BLUE_SPEAKER_POS = new Translation2d(Units.inchesToMeters(-1.50 + 17.5),
+        public static final Translation2d BLUE_SPEAKER_POS = new Translation2d(Units.inchesToMeters(-1.50 + 8),
                 Units.inchesToMeters(218.42));
-        public static final Translation2d RED_SPEAKER_POS = new Translation2d(Units.inchesToMeters(652.73 - 17.5),
+        public static final Translation2d RED_SPEAKER_POS = new Translation2d(Units.inchesToMeters(652.73 - 8),
                 Units.inchesToMeters(218.42));
         public static final double SPEAKER_TO_SPEAKER = Units.inchesToMeters(651.23);
     }
@@ -130,17 +130,7 @@ public static class ShooterConstants {
         public static final double MAX_FLYWHEEL_RPS = 6380.0 / 60;
         public static final double MIN_SHOOTER_DISTANCE = 1.08;
         public static final double MAX_SHOOTER_DISTANCE = 8;
-        public static final double FLYWHEEL_SHUTTLE_SPEED = 0.3;
-
-        //center of red speaker: (652.73 218.42)
-        public static final double RED_X = Units.inchesToMeters(652.73 - 9.05);
-        public static final double RED_Y = Units.inchesToMeters(218.42);
-
-        //center of blue speaker: (-1.50 218.42)
-        public static final double BLUE_X = Units.inchesToMeters(-1.5 + 9.05);
-        public static final double BLUE_Y = Units.inchesToMeters(218.42);
-
-
+        public static final double FLYWHEEL_SHUTTLE_SPEED = 0.3;        
     }
 
     /** Constants for Climb Subsystem. */

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
@@ -18,6 +18,7 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.Constants.SwerveConstants;
 import frc.robot.util.Pose2dSupplier;
 
 import java.util.function.BooleanSupplier;
@@ -214,13 +215,13 @@ public double getShootingDistance() {
         Pose2d currentField = poseSupplier.getPose2d();
 
         if (redSupplier.getAsBoolean()) {  //true = red
-            double xLength = Math.pow(currentField.getX() - ShooterConstants.RED_X, 2);
-            double yLength = Math.pow(currentField.getY() - ShooterConstants.RED_Y, 2);
+            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() - ShooterConstants.BLUE_X, 2);
-            double yLength = Math.pow(currentField.getY() - ShooterConstants.BLUE_Y, 2);
+            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);
         }

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.Constants.SwerveConstants;
 import frc.robot.util.Pose2dSupplier;
 
 import java.util.function.BooleanSupplier;
@@ -133,13 +134,13 @@ private double getShootingDistance() {
         Pose2d currentField = poseSupplier.getPose2d();
 
         if (redSupplier.getAsBoolean()) {  //true = red
-            double xLength = Math.pow(currentField.getX() - ShooterConstants.RED_X, 2);
-            double yLength = Math.pow(currentField.getY() - ShooterConstants.RED_Y, 2);
+            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() - ShooterConstants.BLUE_X, 2);
-            double yLength = Math.pow(currentField.getY() - ShooterConstants.BLUE_Y, 2);
+            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);
         }