From e850bcf8bab8fc986511f081b1b2061b727cddb9 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 27 Aug 2024 00:39:56 -0400 Subject: [PATCH] Revert "Added workaround for switching speaker and amp tag poses in Field.java depending on alliance (only works every time you deploy for some reason)" --- .../com/stuypulse/robot/constants/Field.java | 17 +++-------------- .../com/stuypulse/robot/constants/Settings.java | 4 ++-- .../robot/subsystems/swerve/SwerveDrive.java | 13 ------------- 3 files changed, 5 insertions(+), 29 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index df4a83c..8c0451b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -12,13 +12,10 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import java.util.ArrayList; import java.util.Arrays; -import java.util.Optional; /** This interface stores information about the field elements. */ public interface Field { @@ -33,8 +30,6 @@ public interface Field { double SPEAKER_MAX_HEIGHT = 2.11; // represents the top of the speaker opening double SPEAKER_MIN_HEIGHT = 1.98; // represents the bottom of the speaker opening - Optional alliance = DriverStation.getAlliance(); - public static Pose3d transformToOppositeAlliance(Pose3d pose) { Pose3d rotated = pose.rotateBy(new Rotation3d(0, 0, Math.PI)); @@ -157,8 +152,7 @@ public static AprilTag getTag(int id) { /*** SPEAKER ***/ public static Pose2d getAllianceSpeakerPose() { - // return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER).getLocation().toPose2d(); - return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER ) : (null)) + return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER) .getLocation().toPose2d(); } @@ -172,17 +166,13 @@ public static Pose2d getSpeakerPathFindPose() { /*** AMP ***/ public static Pose2d getAllianceAmpPose() { - return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.BLUE_AMP : NamedTags.RED_AMP ) : (null)) + return (Robot.isBlue() ? NamedTags.BLUE_AMP : NamedTags.RED_AMP) .getLocation().toPose2d(); - // return (Robot.isBlue() ? NamedTags.BLUE_AMP : NamedTags.RED_AMP) - // .getLocation().toPose2d(); } public static Pose2d getOpposingAmpPose() { - return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.RED_AMP : NamedTags.BLUE_AMP ) : (null)) + return (Robot.isBlue() ? NamedTags.RED_AMP : NamedTags.BLUE_AMP) .getLocation().toPose2d(); - // return (Robot.isBlue() ? NamedTags.RED_AMP : NamedTags.BLUE_AMP) - // .getLocation().toPose2d(); } public static Pose2d getAmpPathFindPose() { @@ -209,7 +199,6 @@ public static Pose2d getOpposingSourcePose() { /*** TRAP ***/ public static Pose2d[] getAllianceTrapPoses() { - if (Robot.isBlue()) { return new Pose2d[] { NamedTags.BLUE_STAGE_FAR.getLocation().toPose2d(), diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index eba2cdd..b572d03 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -21,7 +21,7 @@ public interface Settings { double DT = 1.0 / 50.0; - boolean SAFE_MODE_ENABLED = true; + boolean SAFE_MODE_ENABLED = false; double WIDTH = Units.inchesToMeters(36); // intake side double LENGTH = Units.inchesToMeters(32); @@ -309,7 +309,7 @@ public interface Alignment { SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); - SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 20); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); SmartNumber CLIMB_SETUP_DISTANCE = new SmartNumber("Alignment/Climb/Setup Distance", Units.inchesToMeters(21.0)); SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Climb/Into Chain Speed", 0.25); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 5d712ed..0411814 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -255,19 +255,6 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Drive Voltage", Modules[i].getDriveMotor().getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Turn Current", Modules[i].getSteerMotor().getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Turn Voltage", Modules[i].getSteerMotor().getMotorVoltage().getValueAsDouble()); - - // logging for testing field poses on opposite alliances, remove when finished - SmartDashboard.putNumber("Swerve/Speaker Pose X", Field.getAllianceSpeakerPose().getTranslation().getX()); - SmartDashboard.putNumber("Swerve/Speaker Pose Y", Field.getAllianceSpeakerPose().getTranslation().getY()); - SmartDashboard.putNumber("Swerve/Amp Pose X", Field.getAllianceAmpPose().getTranslation().getX()); - SmartDashboard.putNumber("Swerve/Amp Pose Y", Field.getAllianceAmpPose().getTranslation().getY()); - SmartDashboard.putNumber("Swerve/Opposing Amp Pose X", Field.getOpposingAmpPose().getTranslation().getX()); - SmartDashboard.putNumber("Swerve/Opposing Amp Pose Y", Field.getOpposingAmpPose().getTranslation().getY()); - - SmartDashboard.putNumber("Swerve/Current Pose X", SwerveDrive.getInstance().getPose().getX()); - SmartDashboard.putNumber("Swerve/Current Pose Y", SwerveDrive.getInstance().getPose().getY()); - SmartDashboard.putNumber("Swerve/Current Rotation", SwerveDrive.getInstance().getGyroAngle().getDegrees()); - } field.setRobotPose(getPose());