From 0584a07e801a1823e4113c16e42b4d97f147cc51 Mon Sep 17 00:00:00 2001
From: Tyler Wilcox <tyler@tywilly.com>
Date: Thu, 2 Apr 2026 19:29:13 -0400
Subject: [PATCH] exposed more swerve

---
 .../drivesystems/swerve/SwerveDriveSystem.java       | 10 +++++++++-
 .../robolib/drivesystems/swerve/SwerveModule.java    | 12 ++++++++++++
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
index 163846a..0d06c6a 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
@@ -2,6 +2,8 @@ package org.fairportrobotics.frc.robolib.drivesystems.swerve;
 
 import java.util.Arrays;
 
+import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
+
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.hardware.Pigeon2;
@@ -16,6 +18,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 
 public class SwerveDriveSystem implements Subsystem{
@@ -196,7 +201,10 @@ public class SwerveDriveSystem implements Subsystem{
     }
 
     public ChassisSpeeds getRobotRelativeSpeeds(){
-        return this.chassisSpeeds;
+        if(!RobotBase.isReal()){
+            return driveKiniematics.toChassisSpeeds(getRequestedModuleStates());
+        }
+        return driveKiniematics.toChassisSpeeds(getActualModuleStates());
     }
 
     /**
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java
index 98655ce..3b5cb53 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java
@@ -18,6 +18,8 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
+import com.ctre.phoenix6.sim.CANcoderSimState;
+import com.ctre.phoenix6.sim.TalonFXSimState;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -38,11 +40,14 @@ public class SwerveModule {
 
     private TalonFX driveMotor;
     private VelocityVoltage driveRequest = new VelocityVoltage(0);
+    private TalonFXSimState driveSim;
 
     private TalonFX steerMotor;
     private PositionVoltage steerPosRequest = new PositionVoltage(0);
+    private TalonFXSimState steerSim;
 
     private CANcoder steerEncoder;
+    private CANcoderSimState steerEncoderSim;
 
     private String moduleName;
 
@@ -75,16 +80,19 @@ public class SwerveModule {
         steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
         steerEncoder.optimizeBusUtilization();
         steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset));
+        steerEncoderSim = steerEncoder.getSimState();
 
         driveMotor = new TalonFX(driveMotorId, canBusObj);
         driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
         driveMotor.optimizeBusUtilization();
         driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted));
+        driveSim = driveMotor.getSimState();
 
         steerMotor = new TalonFX(steerMotorId, canBusObj);
         steerMotor.optimizeBusUtilization();
         steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA));
         // steerMotor.setNeutralMode(NeutralModeValue.Brake);
+        steerSim = steerMotor.getSimState();
 
         this.moduleLocation = moduleLocation;
 
@@ -167,6 +175,10 @@ public class SwerveModule {
         SmartDashboard.putNumber(moduleName + " steerPos", getSteerRotations().getRotations());
     }
 
+    public void simulationPeriodic(){
+        // driveSim.se
+    }
+
     public String getModuleName(){
         return moduleName;
     }