From 6f233e586fa8b5882c6346750677276c78c82ff7 Mon Sep 17 00:00:00 2001
From: Tyler Wilcox <tyler@wilcoxflp.com>
Date: Fri, 12 Dec 2025 22:52:25 -0500
Subject: [PATCH] Swerve Builder updates

---
 lib/build.gradle                                      |  3 ++-
 .../DriveSystems/SwerveDrive/SwerveBuilder.java       | 10 ++++++++--
 .../SwerveDrive/SwerveDriveSubsystem.java             |  6 +-----
 .../DriveSystems/SwerveDrive/SwerveModule.java        | 11 ++++++++---
 .../org/fairportrobotics/frc/robolib/LibraryTest.java |  2 +-
 5 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/lib/build.gradle b/lib/build.gradle
index 2618b96..7fb47bc 100644
--- a/lib/build.gradle
+++ b/lib/build.gradle
@@ -31,7 +31,8 @@ repositories {
 
 dependencies {
     // Use JUnit Jupiter for testing.
-    testImplementation 'org.junit.jupiter:junit-jupiter:5.8.1'
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 
     // WPI Lib
     implementation "edu.wpi.first.cscore:cscore-java:$wpiVersion"
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java
index 3953e9f..b47f429 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java
@@ -7,7 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d;
 public class SwerveBuilder {
 
     ArrayList<SwerveModule> modules = new ArrayList<SwerveModule>();
-    String canBusName = "";
+    String canBusName = "rio";
 
     private int pigeonId;
 
@@ -49,6 +49,7 @@ public class SwerveBuilder {
         private Translation2d moduleLocation;
         private double gearRatio;
         private double wheelDiameterInMeters;
+        private String moduleName;
 
         public SwerveModuleBuilder() {
 
@@ -114,9 +115,14 @@ public class SwerveBuilder {
             return this;
         }
 
+        public SwerveModuleBuilder withModuleName(String moduleName){
+            this.moduleName = moduleName;
+            return this;
+        }
+
         public SwerveModule build() {
             return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, steerMotorId, steerKP, steerKI, steerKD,
-                    steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters);
+                    steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName);
         }
 
     }
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java
index ce6eb7f..82b7c97 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java
@@ -2,8 +2,6 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive;
 
 import java.util.Arrays;
 
-import org.littletonrobotics.junction.Logger;
-
 import com.ctre.phoenix6.hardware.Pigeon2;
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d;
@@ -44,12 +42,12 @@ public class SwerveDriveSubsystem extends SubsystemBase{
         gyro = new Pigeon2(pigeonId);
 
         poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero);
+        chassisSpeeds = new ChassisSpeeds();
     }
 
     @Override
     public void periodic() {
         SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation);
-        Logger.recordOutput("Un-Optimized Swerve States", moduleStates);
 
         for(int i=0; i<modules.length;i++){
             moduleStates[i].optimize(modules[i].getCurrentSteerRotations());
@@ -57,8 +55,6 @@ public class SwerveDriveSubsystem extends SubsystemBase{
 
         poseEstimator.update(new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
 
-        Logger.recordOutput("Optimized Swerve States", moduleStates);
-
         for(int i=0;i<modules.length;i++){
             modules[i].setModuleState(moduleStates[i]);
         }
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java
index 45266db..6a95ec6 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java
@@ -33,6 +33,8 @@ public class SwerveModule {
     private PositionVoltage steerRequest = new PositionVoltage(0);
     private CANcoder steerEncoder;
 
+    private String moduleName;
+
     private Translation2d moduleLocation;
 
     public SwerveModule(
@@ -49,7 +51,8 @@ public class SwerveModule {
         String canBusName,
         Translation2d moduleLocation,
         double gearRatio,
-        double wheelDiameterInMeters
+        double wheelDiameterInMeters,
+        String moduleName
     ){
         driveMotor = new TalonFX(driveMotorId, canBusName);
         driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
@@ -69,6 +72,8 @@ public class SwerveModule {
 
         this.gearRatio = gearRatio;
         this.wheelDiameterInMeters = wheelDiameterInMeters;
+
+        this.moduleName = moduleName;
     }
 
     public void setSteerRotations(double rotations){
@@ -80,7 +85,7 @@ public class SwerveModule {
     }
 
     public void setDriveSpeed(double speed){
-        double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio * TALON_BUILT_IN_ENCODER_UNITS_PER_ROTATION;
+        double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio;
         driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
     }
 
@@ -106,7 +111,7 @@ public class SwerveModule {
     }
 
     private double getModuleDistance(){
-        double distance = (this.driveMotor.getPosition(true).getValueAsDouble() / TALON_BUILT_IN_ENCODER_UNITS_PER_ROTATION) / gearRatio / (Math.PI * wheelDiameterInMeters);
+        double distance = (this.driveMotor.getPosition(true).getValueAsDouble()) / gearRatio / (Math.PI * wheelDiameterInMeters);
         return distance;
     }
 
diff --git a/lib/src/test/java/org/fairportrobotics/frc/robolib/LibraryTest.java b/lib/src/test/java/org/fairportrobotics/frc/robolib/LibraryTest.java
index 2f80b17..b003bc1 100644
--- a/lib/src/test/java/org/fairportrobotics/frc/robolib/LibraryTest.java
+++ b/lib/src/test/java/org/fairportrobotics/frc/robolib/LibraryTest.java
@@ -1,7 +1,7 @@
 /*
  * This Java source file was generated by the Gradle 'init' task.
  */
-package com.fairportrobotics.frc.robolib;
+package org.fairportrobotics.frc.robolib;
 
 import org.junit.jupiter.api.Test;
 import static org.junit.jupiter.api.Assertions.*;