diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 953b0b75..84a0c372 100644 --- 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 index e63f584c..136290e9 100644 --- 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 index 11dd6a1b..fccf39ac 100644 --- 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); }