diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 01904fd8..6db5e6ca 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -105,6 +105,7 @@ public SwerveSubsystem(File directory) swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. swerveDrive.setAngularVelocityCompensation(true, true, 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. + swerveDrive.setModuleEncoderAutoSynchronize(false, 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. if (visionDriveTest) { setupPhotonVision(); diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index d2f288d7..6fa97228 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -868,6 +868,20 @@ public void setMotorIdleMode(boolean brake) } } + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. + * @param enabled Enable state + * @param deadband Deadband in degrees, default is 3 degrees. + */ + public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband) + { + for(SwerveModule swerveModule : swerveModules) + { + swerveModule.setEncoderAutoSynchronize(enabled, deadband); + } + } + + /** * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index efc3aefe..1fbbe15a 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -112,6 +112,14 @@ public class SwerveModule * Encoder synchronization queued. */ private boolean synchronizeEncoderQueued = false; + /** + * Encoder <- Absolute encoder synchronization enabled. + */ + private boolean synchronizeEncoderEnabled = false; + /** + * Encoder synchronization deadband in degrees. + */ + private double synchronizeEncoderDeadband = 3; /** @@ -242,12 +250,32 @@ public void setDriveMotorVoltageCompensation(double optimalVoltage) */ public void queueSynchronizeEncoders() { - if (absoluteEncoder != null) + if (absoluteEncoder != null && synchronizeEncoderEnabled) { synchronizeEncoderQueued = true; } } + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. + * @param enabled Enable state + * @param deadband Deadband in degrees, default is 3 degrees. + */ + public void setEncoderAutoSynchronize(boolean enabled, double deadband) + { + synchronizeEncoderEnabled = enabled; + synchronizeEncoderDeadband = deadband; + } + + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. + * @param enabled Enable state + */ + public void setEncoderAutoSynchronize(boolean enabled) + { + synchronizeEncoderEnabled = enabled; + } + /** * Set the antiJitter functionality, if true the modules will NOT auto center. Pushes the offsets to the angle motor * controllers as well. @@ -355,10 +383,12 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, // Prevent module rotation if angle is the same as the previous angle. // Synchronize encoders if queued and send in the current position as the value from the absolute encoder. - if (absoluteEncoder != null && synchronizeEncoderQueued) + if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled) { double absoluteEncoderPosition = getAbsolutePosition(); - angleMotor.setPosition(absoluteEncoderPosition); + if(Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) { + angleMotor.setPosition(absoluteEncoderPosition); + } angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); synchronizeEncoderQueued = false; } else