From 884bb1aa53970d29b6ba85b9e416722f32660925 Mon Sep 17 00:00:00 2001
From: Alvin Zhang <41vin2h4n9@gmail.com>
Date: Wed, 10 Jan 2024 17:33:55 -0800
Subject: [PATCH] fix: module offsets

---
 src/main/java/org/team1540/robot2024/Robot.java       |  1 -
 .../java/org/team1540/robot2024/RobotContainer.java   | 11 +++++------
 .../team1540/robot2024/subsystems/drive/Module.java   |  1 -
 .../team1540/robot2024/util/swerve/SwerveFactory.java |  8 ++++----
 4 files changed, 9 insertions(+), 12 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index f86b2365..a6c91784 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -113,7 +113,6 @@ public void disabledPeriodic() {
     @Override
     public void autonomousInit() {
         autonomousCommand = robotContainer.getAutonomousCommand();
-        robotContainer.drive.setPose(PathPlannerAuto.getStaringPoseFromAutoFile("TestAuto"));
         // schedule the autonomous command (example)
         if (autonomousCommand != null) {
             autonomousCommand.schedule();
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
index 2c1f2b53..450ed200 100644
--- a/src/main/java/org/team1540/robot2024/RobotContainer.java
+++ b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -42,10 +42,10 @@ public RobotContainer() {
                 drive =
                         new Drive(
                                 new GyroIONavx(),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(1, SwerveFactory.SwerveCorner.FRONT_LEFT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(7, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(4, SwerveFactory.SwerveCorner.BACK_LEFT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(3, SwerveFactory.SwerveCorner.BACK_RIGHT)));
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(3, SwerveFactory.SwerveCorner.FRONT_LEFT)),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(4, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(7, SwerveFactory.SwerveCorner.BACK_LEFT)),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(1, SwerveFactory.SwerveCorner.BACK_RIGHT)));
                 // drive = new Drive(
                 // new GyroIOPigeon2(),
                 // new ModuleIOTalonFX(0),
@@ -59,8 +59,7 @@ public RobotContainer() {
                 // Sim robot, instantiate physics sim IO implementations
                 drive =
                         new Drive(
-                                new GyroIO() {
-                                },
+                                new GyroIO() {},
                                 new ModuleIOSim(),
                                 new ModuleIOSim(),
                                 new ModuleIOSim(),
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
index 63c6288e..9e4d2d62 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
@@ -53,7 +53,6 @@ public Module(ModuleIO io, int index) {
     }
 
     public void periodic() {
-        System.out.println(driveFeedback.getP() + " " + driveFeedback.getI() + " " + driveFeedback.getD());
         io.updateInputs(inputs);
         Logger.processInputs("Drive/Module" + index, inputs);
 
diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
index 7baeab32..977d55a6 100644
--- a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
+++ b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
@@ -12,9 +12,9 @@
 
 public class SwerveFactory {
     private static final double[] moduleOffsetsRots = new double[]{
-            -0.4130859, // Module 1
+            -0.9130859, // Module 1
             0.0, // Module 2
-            -0.2197265, // Module 3
+            -0.7197265, // Module 3
             -0.7722, // Module 4
             0.0, // Module 5
             0.0, // Module 6
@@ -27,10 +27,10 @@ public static SwerveModuleHW getModuleMotors(int id, SwerveCorner corner) {
     }
 
     public enum SwerveCorner {
-        FRONT_LEFT(180),
+        FRONT_LEFT(0),
         FRONT_RIGHT(90),
         BACK_LEFT(270),
-        BACK_RIGHT(0);
+        BACK_RIGHT(180);
 
         private final double offset;
         SwerveCorner(double offset) {