From ae742134fec8c62eb2e8816f844068d11c8f018f Mon Sep 17 00:00:00 2001
From: Tyler Wilcox <tyler@tywilly.com>
Date: Tue, 27 Jan 2026 20:46:13 -0500
Subject: [PATCH] Working logically

---
 .../SwerveDrive/SwerveBuilder.java            | 16 +++++-
 .../SwerveDrive/SwerveDriveSubsystem.java     | 10 ++--
 .../SwerveDrive/SwerveModule.java             | 53 ++++++++++++-------
 3 files changed, 54 insertions(+), 25 deletions(-)

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 4b5781a..66ba603 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
@@ -54,7 +54,7 @@ public class SwerveBuilder {
     }
 
     public SwerveDriveSubsystem build(){
-        return new SwerveDriveSubsystem(pigeonId, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size]));
+        return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size]));
     }
 
     public class SwerveModuleBuilder {
@@ -63,10 +63,12 @@ public class SwerveBuilder {
         private double driveKP;
         private double driveKI;
         private double driveKD;
+        private double driveKV;
         private int steerMotorId;
         private double steerKP;
         private double steerKI;
         private double steerKD;
+        private double steerKV;
         private int steerEncoderId;
         private double steerOffset;
         private Translation2d moduleLocation;
@@ -98,6 +100,11 @@ public class SwerveBuilder {
             return this;
         }
 
+        public SwerveModuleBuilder withDriveKV(double driveKV){
+            this.driveKV = driveKV;
+            return this;
+        }
+
         public SwerveModuleBuilder withSteerMotorId(int steerMotorId){
             this.steerMotorId = steerMotorId;
             return this;
@@ -118,6 +125,11 @@ public class SwerveBuilder {
             return this;
         }
 
+        public SwerveModuleBuilder withSteerKV(double steerKV){
+            this.steerKV = steerKV;
+            return this;
+        }
+
         public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId){
             this.steerEncoderId = steerEncoderId;
             return this;
@@ -144,7 +156,7 @@ public class SwerveBuilder {
         }
 
         public SwerveModule build() {
-            return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, steerMotorId, steerKP, steerKI, steerKD,
+            return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, steerKD, steerKV,
                     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 13e196d..5f65f70 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
@@ -34,7 +34,7 @@ public class SwerveDriveSubsystem extends SubsystemBase{
 
     private double mLastSimTime = 0;
 
-    public SwerveDriveSubsystem(int pigeonId, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
+    public SwerveDriveSubsystem(int pigeonId, String canBusName, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
 
         MAX_LINEAR_SPEED = maxLinearVelMetersSecond;
         MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond;
@@ -45,7 +45,7 @@ public class SwerveDriveSubsystem extends SubsystemBase{
             Arrays.stream(modules).map((m) -> m.getModuleLocation()).toArray(size -> new Translation2d[size])
         );
 
-        gyro = new Pigeon2(pigeonId);
+        gyro = new Pigeon2(pigeonId, canBusName);
 
         poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero);
         chassisSpeeds = new ChassisSpeeds();
@@ -56,9 +56,9 @@ public class SwerveDriveSubsystem extends SubsystemBase{
         super.periodic();
         SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation);
 
-        for(int i=0; i<modules.length;i++){
-            moduleStates[i].optimize(modules[i].getSteerRotations());
-        }
+        // for(int i=0; i<modules.length;i++){
+        //     moduleStates[i].optimize(modules[i].getSteerRotations());
+        // }
 
         poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
 
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 420b4c9..54a54de 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
@@ -11,12 +11,14 @@ import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
-import com.sun.jdi.request.StepRequest;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.measure.Angle;
 
 public class SwerveModule {
 
@@ -45,10 +47,12 @@ public class SwerveModule {
         double driveKP,
         double driveKI,
         double driveKD,
+        double driveKV,
         int steerMotorId,
         double steerKP,
         double steerKI,
         double steerKD,
+        double steerKV,
         int steerEncoderId,
         double steerOffset,
         String canBusName,
@@ -57,20 +61,21 @@ public class SwerveModule {
         double wheelDiameterInMeters,
         String moduleName
     ){
-        driveMotor = new TalonFX(driveMotorId, canBusName);
-        driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
-        driveMotor.optimizeBusUtilization();
-        driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD));
-
-        steerMotor = new TalonFX(steerMotorId, canBusName);
-        steerMotor.optimizeBusUtilization();
-        steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD));
 
         steerEncoder = new CANcoder(steerEncoderId, canBusName);
         steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
         steerEncoder.optimizeBusUtilization();
         steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset));
 
+        driveMotor = new TalonFX(driveMotorId, canBusName);
+        driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
+        driveMotor.optimizeBusUtilization();
+        driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV));
+
+        steerMotor = new TalonFX(steerMotorId, canBusName);
+        steerMotor.optimizeBusUtilization();
+        steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKV));
+
         this.moduleLocation = moduleLocation;
 
         this.gearRatio = gearRatio;
@@ -79,7 +84,7 @@ public class SwerveModule {
         this.moduleName = moduleName;
     }
 
-    public void setSteerRotations(double rotations){
+    public void setSteerRotations(Angle rotations){
         steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations));
     }
 
@@ -90,7 +95,7 @@ public class SwerveModule {
     public void setDriveSpeed(double speed){
         // Convert wheel speed to rotor speed
         // Circumfrence of wheel / speed * gear ratio
-        double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio;
+        double rotorVelocity = (speed / (Math.PI * wheelDiameterInMeters)) * gearRatio;
         driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
     }
 
@@ -107,7 +112,7 @@ public class SwerveModule {
      * @return
      */
     public double getDriveWheelSpeed(){
-        return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters);
+        return (driveMotor.getVelocity().refresh().getValueAsDouble() * gearRatio) / (Math.PI * wheelDiameterInMeters);
     }
 
     public Translation2d getModuleLocation(){
@@ -115,7 +120,7 @@ public class SwerveModule {
     }
 
     public void setModuleState(SwerveModuleState state){
-        this.setSteerRotations(state.angle.getRotations());
+        this.setSteerRotations(state.angle.getMeasure());
         this.setDriveSpeed(state.speedMetersPerSecond);
         moduleState = state;
     }
@@ -133,34 +138,46 @@ public class SwerveModule {
     }
 
     private double getModuleDistance(){
-        double distance = (this.driveMotor.getPosition(true).getValueAsDouble()) / gearRatio * (Math.PI * wheelDiameterInMeters);
+        double distance = (this.driveMotor.getPosition(true).getValueAsDouble() * gearRatio) / (Math.PI * wheelDiameterInMeters);
         return distance;
     }
 
-    private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD){
+    private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV){
         return new TalonFXConfiguration().withSlot0(
             new Slot0Configs()
                 .withKP(kP)
                 .withKI(kI)
                 .withKD(kD)
+                .withKV(kV)
             );
     }
 
-    private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD){
+    private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kV){
         return new TalonFXConfiguration().withSlot0(
             new Slot0Configs()
                 .withKP(kP)
                 .withKI(kI)
                 .withKD(kD)
+                .withKS(kV)
+                .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)
         ).withFeedback(
             new FeedbackConfigs()
                 .withFeedbackRemoteSensorID(steerEncoder.getDeviceID())
-                .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
+                .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder)
+                .withSensorToMechanismRatio(1.0)
+                .withRotorToSensorRatio(12.8)
+        ).withClosedLoopGeneral(
+            new ClosedLoopGeneralConfigs()
+            .withContinuousWrap(true)
         );
     }
 
     private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){
-        return new CANcoderConfiguration().withMagnetSensor(new MagnetSensorConfigs().withMagnetOffset(steerOffset));
+        return new CANcoderConfiguration().withMagnetSensor(
+            new MagnetSensorConfigs()
+                .withMagnetOffset(steerOffset)
+                .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
+            );
     }
 
 }