+10 points for nick

Author: ohowe1

src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java

@@ -303,15 +303,15 @@ public Command steerDynamicSysIDCommand(SysIdRoutine.Direction direction) {
 
   @Override
   public void periodic() {
-    logValues();
-
     List<EstimatedRobotPose> estimatedRobotPoses = cameraPoseDataSupplier.apply(getPose());
     for (EstimatedRobotPose estimatedRobotPose : estimatedRobotPoses) {
       if (!DriverStation.isAutonomousEnabled()) {
         poseEstimator.addVisionMeasurement(
             estimatedRobotPose.estimatedPose.toPose2d(), estimatedRobotPose.timestampSeconds);
       }
     }
+
+    logValues();
   }
 
   private void logValues() {

src/main/java/frc/robot/subsystems/swerve/SwerveModule.java

@@ -389,10 +389,11 @@ public void setDesiredState(SwerveModuleState state, boolean activeSteer, boolea
 
     controlModeEntry.append(SwerveModuleControlMode.NORMAL.logValue);
 
-    state = SwerveModuleState.optimize(state, getSteerAngle());
+    Rotation2d steerAngle = getSteerAngle();
+    state = SwerveModuleState.optimize(state, steerAngle);
 
     // Account for steer error to reduce skew
-    double steerErrorRadians = state.angle.getRadians() - getSteerAngle().getRadians();
+    double steerErrorRadians = state.angle.getRadians() - steerAngle.getRadians();
     double cosineScalar = Math.cos(steerErrorRadians);
     if (cosineScalar < 0.0) {
       cosineScalar = 0.0;
@@ -430,7 +431,7 @@ private void setSteerReference(double targetAngleRadians, boolean activeSteer) {
 
     if (activeSteer) {
       steerMotor.setControl(
-          steerMotionMagicExpoVoltage.withPosition(targetAngleRadians * steerConversion));
+          steerMotionMagicExpoVoltage.withPosition(targetAngleRadians / steerConversion));
     } else {
       steerMotor.setVoltage(0.0);
     }