Hopeful fixes from the first round of tests

lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java

@@ -9,6 +9,9 @@ public class SwerveBuilder {
     ArrayList<SwerveModule> modules = new ArrayList<SwerveModule>();
     String canBusName = "rio";
 
+    private double maxAngularVelRadiansSecond = 1.0;
+    private double maxLinearVelMetersSecond = Math.PI;
+
     private int pigeonId;
 
     public SwerveBuilder(){
@@ -30,8 +33,28 @@ public class SwerveBuilder {
         return this;
     }
 
+    /**
+     * Set the max linear velocity in meters per second
+     * @param maxVel Max linear velocity in meters per second
+     * @return The swerve builder
+     */
+    public SwerveBuilder withMaxLinearVelocity(double maxVel){
+        this.maxLinearVelMetersSecond = maxVel;
+        return this;
+    }
+
+    /**
+     * Set the max angular velocity in radians per second
+     * @param maxVel Max angular velocity in radians per second
+     * @return The swerve builder
+     */
+    public SwerveBuilder withMaxAngularVelocity(double maxVel){
+        this.maxAngularVelRadiansSecond = maxVel;
+        return this;
+    }
+
     public SwerveDriveSubsystem build(){
-        return new SwerveDriveSubsystem(pigeonId, modules.toArray(size -> new SwerveModule[size]));
+        return new SwerveDriveSubsystem(pigeonId, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size]));
     }
 
     public class SwerveModuleBuilder {

lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java

@@ -2,6 +2,7 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive;
 
 import java.util.Arrays;
 
+import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.hardware.Pigeon2;
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d;
@@ -26,12 +27,17 @@ public class SwerveDriveSubsystem extends SubsystemBase{
 
     private SwerveModule[] modules;
 
-    private final double MAX_LINEAR_SPEED = 3.9;
+    private double MAX_LINEAR_SPEED = 3.9;
-    private final double MAX_ANGULAR_SPEED = Math.PI * 2;
+    private double MAX_ANGULAR_SPEED = Math.PI * 2;
 
     private Pigeon2 gyro;
 
-    public SwerveDriveSubsystem(int pigeonId, SwerveModule... modules){
+    private double mLastSimTime = 0;
+
+    public SwerveDriveSubsystem(int pigeonId, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
+
+        MAX_LINEAR_SPEED = maxLinearVelMetersSecond;
+        MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond;
 
         this.modules = modules; 
 
@@ -47,19 +53,34 @@ public class SwerveDriveSubsystem extends SubsystemBase{
 
     @Override
     public void periodic() {
+        super.periodic();
         SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation);
 
         for(int i=0; i<modules.length;i++){
-            moduleStates[i].optimize(modules[i].getCurrentSteerRotations());
+            moduleStates[i].optimize(modules[i].getSteerRotations());
         }
 
-        poseEstimator.update(new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
+        poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
 
         for(int i=0;i<modules.length;i++){
             modules[i].setModuleState(moduleStates[i]);
         }
     }
 
+    @Override
+    public void simulationPeriodic() {
+        super.simulationPeriodic();
+
+        double deltaTimeSecond = (mLastSimTime - Utils.getCurrentTimeSeconds());
+
+        if(mLastSimTime != 0){
+            Pose3d oldPose = getRobotPose();
+            poseEstimator.resetPose(new Pose3d(oldPose.getX() + (chassisSpeeds.vxMetersPerSecond * (deltaTimeSecond)), oldPose.getY() + chassisSpeeds.vyMetersPerSecond * (deltaTimeSecond), 0, oldPose.getRotation().plus(new Rotation3d(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * (deltaTimeSecond))))));
+        }
+
+        mLastSimTime = Utils.getCurrentTimeSeconds();
+    }
+
     public Translation2d[] getModuleLocations(){
         return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleLocation()).toArray(Translation2d[]::new);
     }
@@ -68,6 +89,14 @@ public class SwerveDriveSubsystem extends SubsystemBase{
         return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new);
     }
 
+    public SwerveModule[] getModules(){
+        return Arrays.stream(modules).toArray(SwerveModule[]::new);
+    }
+
+    public int getNumberOfModules(){
+        return modules.length;
+    }
+
     public Angle getCurrentYaw(){
         return gyro.getYaw().refresh().getValue();
     }
@@ -81,7 +110,13 @@ public class SwerveDriveSubsystem extends SubsystemBase{
         this.centerOfRotation = centerOfRotation;
     }
 
-    public void setChassisSpeedsFromJoystick(double x, double y, double rot){
+    /**
+     * Set robot speed relative to field
+     * @param x Forward robot speed in meters per second
+     * @param y Left robot speed in meters per second
+     * @param rot CCW robot speed in radians per second
+     */
+    public void setChassisSpeedsFromJoystickFieldRelative(double x, double y, double rot){
         this.setChassisSpeed(ChassisSpeeds.fromFieldRelativeSpeeds(
             MAX_LINEAR_SPEED * x,
             MAX_LINEAR_SPEED * y,
@@ -90,6 +125,25 @@ public class SwerveDriveSubsystem extends SubsystemBase{
         ));
     }
 
+    /**
+     * Set robot speed relative to robot
+     * @param x Forward robot speed in meters per second
+     * @param y Left robot speed in meters per second
+     * @param rot CCW robot speed in radians per second
+     */
+    public void setChassisSpeedsFromJoystickRobotRelative(double x, double y, double rot){
+        this.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(
+            MAX_LINEAR_SPEED * x,
+            MAX_LINEAR_SPEED * y,
+            MAX_ANGULAR_SPEED * rot,
+            Rotation2d.fromDegrees(getCurrentYaw().magnitude())
+        ));
+    }
+
+    public Pose3d getRobotPose(){
+        return poseEstimator.getEstimatedPosition();
+    }
+
     private SwerveModulePosition[] getModulePositions(){
         return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new);
     }

lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java

@@ -1,6 +1,8 @@
 package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive;
 
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -8,6 +10,7 @@ import com.ctre.phoenix6.controls.PositionVoltage;
 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 edu.wpi.first.math.geometry.Rotation2d;
@@ -57,11 +60,11 @@ public class SwerveModule {
         driveMotor = new TalonFX(driveMotorId, canBusName);
         driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
         driveMotor.optimizeBusUtilization();
-        driveMotor.getConfigurator().apply(generateTalonConfiguration(driveKP, driveKI, driveKD));
+        driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD));
 
         steerMotor = new TalonFX(steerMotorId, canBusName);
         steerMotor.optimizeBusUtilization();
-        steerMotor.getConfigurator().apply(generateTalonConfiguration(steerKP, steerKI, steerKD));
+        steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD));
 
         steerEncoder = new CANcoder(steerEncoderId, canBusName);
         steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
@@ -80,19 +83,33 @@ public class SwerveModule {
         steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations));
     }
 
-    public Rotation2d getCurrentSteerRotations(){
+    public Rotation2d getSteerRotations(){
         return new Rotation2d(steerEncoder.getPosition().refresh().getValueAsDouble());
     }
 
     public void setDriveSpeed(double speed){
+        // Convert wheel speed to rotor speed
+        // Circumfrence of wheel / speed * gear ratio
         double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio;
-        //driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
+        driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
     }
 
-    public double getDriveSpeed(){
+    /**
+     * Get drive motor speed in rotations per second
+     * @return
+     */
+    public double getDriveMotorSpeed(){
         return driveMotor.getVelocity().refresh().getValueAsDouble();
     }
 
+    /**
+     * Get drive wheel speed in meters per second
+     * @return
+     */
+    public double getDriveWheelSpeed(){
+        return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters);
+    }
+
     public Translation2d getModuleLocation(){
         return moduleLocation;
     }
@@ -108,16 +125,38 @@ public class SwerveModule {
     }
 
     public SwerveModulePosition getModulePosition(){
-        return new SwerveModulePosition(this.getModuleDistance(), getCurrentSteerRotations());
+        return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations());
+    }
+
+    public String getModuleName(){
+        return moduleName;
     }
 
     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 generateTalonConfiguration(double kP, double kI, double kD){
+    private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD){
-        return new TalonFXConfiguration().withSlot0(new Slot0Configs().withKP(kP).withKI(kI).withKD(kD));
+        return new TalonFXConfiguration().withSlot0(
+            new Slot0Configs()
+                .withKP(kP)
+                .withKI(kI)
+                .withKD(kD)
+            );
+    }
+
+    private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD){
+        return new TalonFXConfiguration().withSlot0(
+            new Slot0Configs()
+                .withKP(kP)
+                .withKI(kI)
+                .withKD(kD)
+        ).withFeedback(
+            new FeedbackConfigs()
+                .withFeedbackRemoteSensorID(steerEncoder.getDeviceID())
+                .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
+        );
     }
 
     private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){