Revert "Added workaround for switching speaker and amp tag poses in F…

…ield.java depending on alliance (only works every time you deploy for some reason)"
StuyPulse · Aug 27, 2024 · e850bcf · e850bcf
1 parent 5526db1
commit e850bcf
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 29 deletions.
diff --git 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> 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
@@ -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
@@ -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());