diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 2955710..47b300b 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.Subsystem; import frc.lib.Telemetry; @@ -94,6 +95,8 @@ public void addToShuffleboard(ShuffleboardTab tab) { "Setpoint Angle (deg)", () -> swerveModule.getSetpoint().angle.getDegrees()); swerveModuleColumn.addDouble( "Setpoint Velocity (mps)", () -> swerveModule.getSetpoint().speedMetersPerSecond); + + swerveModuleColumn.add(Commands.runOnce(swerveModule::syncSteerPosition).withName("Sync Steer")); } } diff --git a/src/main/java/frc/robot/swerve/SwerveModuleIO.java b/src/main/java/frc/robot/swerve/SwerveModuleIO.java index 991f791..1980443 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleIO.java @@ -7,6 +7,11 @@ /** Swerve module hardware interface. */ public interface SwerveModuleIO { + /** + * Sets the steer motor's position to the azimuth encoder's position. + */ + public void syncSteerPosition(); + /** * Gets the state of the swerve module. * diff --git a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java index ac60820..80db9ec 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java @@ -52,6 +52,11 @@ public SwerveModuleIOCustom(SwerveModuleConfig config) { setpoint = new SwerveModuleState(); + syncSteerPosition(); + } + + @Override + public void syncSteerPosition() { azimuthEncoder.update(azimuthEncoderValues); steerMotor.setPosition(azimuthEncoderValues.positionRotations); }