From 67090231910a2c7d518beac0691b5492e79677c4 Mon Sep 17 00:00:00 2001
From: BenG49 <ben@goldfisher.com>
Date: Tue, 23 Jan 2024 14:38:47 -0500
Subject: [PATCH] Make forwards intake side

---
 .../java/com/stuypulse/robot/RobotContainer.java     |  8 ++++----
 .../java/com/stuypulse/robot/constants/Ports.java    |  8 ++++----
 .../java/com/stuypulse/robot/constants/Settings.java | 12 ++++--------
 .../robot/subsystems/odometry/Odometry.java          |  2 ++
 4 files changed, 14 insertions(+), 16 deletions(-)

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
index bb964b4..330b875 100644
--- a/src/main/java/com/stuypulse/robot/RobotContainer.java
+++ b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -74,10 +74,10 @@ private void configureDefaultCommands() {
     /***************/
 
     private void configureButtonBindings() {
-        driver.getDPadUp().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(180)));
-        driver.getDPadDown().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(0)));
-        driver.getDPadLeft().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(270)));
-        driver.getDPadRight().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(90)));
+        driver.getDPadUp().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(0)));
+        driver.getDPadDown().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(180)));
+        driver.getDPadLeft().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(90)));
+        driver.getDPadRight().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(270)));
         
         driver.getRightTriggerButton()
             .whileTrue(new IntakeAcquire())
diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java
index 0cb161a..4647e2d 100644
--- a/src/main/java/com/stuypulse/robot/constants/Ports.java
+++ b/src/main/java/com/stuypulse/robot/constants/Ports.java
@@ -14,25 +14,25 @@ public interface Gamepad {
     }
 
     public interface Swerve {
-        public interface FrontRight {
+        public interface BackLeft {
             int DRIVE = 10;
             int TURN = 11;
             int ENCODER = 2;
         }
 
-        public interface FrontLeft {
+        public interface BackRight {
             int DRIVE = 12;
             int TURN = 13;
             int ENCODER = 1;
         }
         
-        public interface BackLeft {
+        public interface FrontRight {
             int DRIVE = 14;
             int TURN = 15;
             int ENCODER = 4;
         }
         
-        public interface BackRight {
+        public interface FrontLeft {
             int DRIVE = 16;
             int TURN = 17;
             int ENCODER = 3;
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
index c29433a..d03e8e3 100644
--- a/src/main/java/com/stuypulse/robot/constants/Settings.java
+++ b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -80,29 +80,25 @@ public interface Motion {
         
  		public interface FrontRight {
             String ID = "Front Right";
-            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(65.566406);
-                // .plus(Rotation2d.fromDegrees(0));
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(208.212891 + 180);
             Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5);
         }
 
         public interface FrontLeft {
             String ID = "Front Left";
-            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(47.197266);
-                // .plus(Rotation2d.fromDegrees(270));
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(154.511719 + 180);
             Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * +0.5);
         }
 
         public interface BackLeft {
             String ID = "Back Left";
-            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(208.212891);
-                // .plus(Rotation2d.fromDegrees(180));
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(65.566406 + 180);
             Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * +0.5);
         }
 
         public interface BackRight {
             String ID = "Back Right";
-            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(154.511719);
-                // .plus(Rotation2d.fromDegrees(90));
+            Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(47.197266 + 180);
             Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5);
         }
 
diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java
index d4312c2..e3ef34e 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java
@@ -99,6 +99,8 @@ public void periodic() {
 
         odometryPose2D.setPose(odometry.getPoseMeters());
         estimatorPose2D.setPose(estimator.getEstimatedPosition());
+
+        updateTelemetry();
     }
 
     @Override