diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index eefada5..31ea182 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -156,15 +156,6 @@ public class SwerveModule desiredState = new SwerveModuleState2( simpleState.speedMetersPerSecond, simpleState.angle, desiredState.omegaRadPerSecond); - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) - { - SmartDashboard.putNumber( - "Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond); - SmartDashboard.putNumber( - "Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees()); - SmartDashboard.putNumber( - "Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond)); - } if (isOpenLoop) { @@ -195,6 +186,16 @@ public class SwerveModule angle += 360; } + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SmartDashboard.putNumber( + "Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond); + SmartDashboard.putNumber( + "Optimized " + moduleNumber + " Angle Setpoint: ", angle); + SmartDashboard.putNumber( + "Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond)); + } + // Prevent module rotation if angle is the same as the previous angle. if (angle != lastAngle || synchronizeEncoderQueued) {