From 436667d0e4c7002d5f8ed38e0e4c15d0464e91cf Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Tue, 27 Jan 2026 18:34:34 -0500
Subject: [PATCH] Pushing up the little bit of work from 1/27 build session

---
 .../robot/constants/DrivetrainConstants.java  | 37 +++++++------------
 .../java/frc/robot/subsystems/Drivetrain.java | 16 ++++----
 2 files changed, 22 insertions(+), 31 deletions(-)

diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java
index 10749f7..450ee6e 100644
--- a/src/main/java/frc/robot/constants/DrivetrainConstants.java
+++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java
@@ -1,5 +1,6 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.util.Units;
@@ -11,36 +12,26 @@ public class DrivetrainConstants {
 
     public static final double kTrackWidth = Units.inchesToMeters(23.75);
     public static final double kWheelBase = Units.inchesToMeters(18.75);
+ 
+    public static final double kFrontLeftMagEncoderOffset = 2.98;
+    public static final double kFrontRightMagEncoderOffset = 1.18;
+    public static final double kRearLeftMagEncoderOffset = 3.69;
+    public static final double kRearRightMagEncoderOffset = 2.54;
 
-    // TODO Replace zeros with real numbers
-    // These values should be determinable by writing the magnetic encoder output
-    // to the dashboard, and manually aligning all wheels such that the bevel gears
-    // on the side of the wheel all face left. Some known straight edge (like 1x1 or similar)
-    // should be used as the alignment medium. If done correctly, and the modules aren't disassembled,
-    // then these values should work throughout the season the first time they are set. 
-    public static final double kFrontLeftMagEncoderOffset = 0;
-    public static final double kFrontRightMagEncoderOffset = 0;
-    public static final double kRearLeftMagEncoderOffset = 0;
-    public static final double kRearRightMagEncoderOffset = 0;
-
-    // Kraken CAN IDs
     public static final int kFrontLeftDrivingCANID = 0;
-    public static final int kFrontRightDrivingCANID = 1;//3;
-    public static final int kRearLeftDrivingCANID = 3;//1;
+    public static final int kFrontRightDrivingCANID = 3;
+    public static final int kRearLeftDrivingCANID = 1;
     public static final int kRearRightDrivingCANID = 2;
 
-    // SparkMAX CAN IDs
     public static final int kFrontLeftTurningCANID = 8;
-    public static final int kFrontRightTurningCANID = 7;//9;
-    public static final int kRearLeftTurningCANID = 9;//7;
+    public static final int kFrontRightTurningCANID = 9;
+    public static final int kRearLeftTurningCANID = 7;
     public static final int kRearRightTurningCANID = 6;
 
-    // Analog Encoder Input Ports 
-    // TODO Real Port IDs
-    public static final int kFrontLeftAnalogInPort = 0;
-    public static final int kFrontRightAnalogInPort = 1;
-    public static final int kRearLeftAnalogInPort = 2;
-    public static final int kRearRightAnalogInPort = 3;
+    public static final int kFrontLeftAnalogInPort = 3;
+    public static final int kFrontRightAnalogInPort = 2;
+    public static final int kRearLeftAnalogInPort = 0;
+    public static final int kRearRightAnalogInPort = 1;
 
     public static final boolean kGyroReversed = true;
 
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 81a5413..335d20e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -48,34 +48,34 @@ public class Drivetrain extends SubsystemBase {
         frontLeft = new SwerveModule(
             "FrontLeft", 
             DrivetrainConstants.kFrontLeftDrivingCANID, 
-            DrivetrainConstants.kFrontLeftTurningCANID);/*, 
+            DrivetrainConstants.kFrontLeftTurningCANID, 
             DrivetrainConstants.kFrontLeftAnalogInPort, 
             DrivetrainConstants.kFrontLeftMagEncoderOffset
-        ); */
+        ); 
 
         frontRight = new SwerveModule(
             "FrontRight", 
             DrivetrainConstants.kFrontRightDrivingCANID, 
-            DrivetrainConstants.kFrontRightTurningCANID);/*, 
+            DrivetrainConstants.kFrontRightTurningCANID, 
             DrivetrainConstants.kFrontRightAnalogInPort, 
             DrivetrainConstants.kFrontRightMagEncoderOffset
-        );*/
+        );
 
         rearLeft = new SwerveModule(
             "RearLeft", 
             DrivetrainConstants.kRearLeftDrivingCANID, 
-            DrivetrainConstants.kRearLeftTurningCANID);/*, 
+            DrivetrainConstants.kRearLeftTurningCANID, 
             DrivetrainConstants.kRearLeftAnalogInPort, 
             DrivetrainConstants.kRearLeftMagEncoderOffset
-        );*/
+        );
 
         rearRight = new SwerveModule(
             "RearRight", 
             DrivetrainConstants.kRearRightDrivingCANID, 
-            DrivetrainConstants.kRearRightTurningCANID);/*, 
+            DrivetrainConstants.kRearRightTurningCANID, 
             DrivetrainConstants.kRearRightAnalogInPort, 
             DrivetrainConstants.kRearRightMagEncoderOffset
-        );*/
+        );
 
         gyro = new AHRS(NavXComType.kMXP_SPI);