diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index c28b7ec..35d13d8 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -11,6 +11,7 @@ import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.util.Units; public class ModuleConstants { @@ -50,6 +51,9 @@ public class ModuleConstants { public static final double kTurnP = 1; public static final double kTurnI = 0; public static final double kTurnD = 0; + + // TODO How sensitive is too sensitive? + public static final double kAutoResetPositionDeadband = Units.degreesToRadians(.25); public static final int kTurnMotorCurrentLimit = 20; @@ -98,5 +102,9 @@ public class ModuleConstants { .outputRange(-1, 1) .positionWrappingEnabled(true) .positionWrappingInputRange(0, 2 * Math.PI); + turningConfig.signals + .primaryEncoderVelocityAlwaysOn(false) + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20); } } diff --git a/src/main/java/frc/robot/utilities/SwerveModule.java b/src/main/java/frc/robot/utilities/SwerveModule.java index 06f1c04..2447c90 100644 --- a/src/main/java/frc/robot/utilities/SwerveModule.java +++ b/src/main/java/frc/robot/utilities/SwerveModule.java @@ -43,7 +43,10 @@ public class SwerveModule { private String moduleName; + private SwerveModuleState lastTargetState; + private boolean isAbsoluteEncoderDisabled; + private boolean turningEncoderAutoRezeroEnabled; /** * Builds the swerve module but with the Absolute Encoder disabled. @@ -60,7 +63,8 @@ public class SwerveModule { } /** - * Builds the swerve module with the normal features + * Builds the swerve module with the normal features, disables automatic rezeroing of the turning encoder + * from the absolute encoder. * * @param moduleName The module name, Front Left, Front Right, etc. * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel @@ -69,6 +73,22 @@ public class SwerveModule { * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module */ public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) { + this(moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false); + } + + /** + * Builds the swerve module with the normal features, and gives the option to enable automatic turning encoder rezeroing + * when the turning motor is not moving + * + * @param moduleName The module name, Front Left, Front Right, etc. + * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel + * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel + * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder + * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module + * @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder + */ + public SwerveModule(String moduleName, int drivingCANID, int turningCANID, + int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) { isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0); drive = new TalonFX(drivingCANID); @@ -102,6 +122,10 @@ public class SwerveModule { drive.setPosition(0); + this.lastTargetState = getState(); + + this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled; + this.moduleName = "Drivetrain/Modules/" + moduleName; } @@ -109,6 +133,12 @@ public class SwerveModule { Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get()); Logger.recordOutput(moduleName + "/SwerveModuleState", getState()); Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition()); + + if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) { + if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) { + resetEncoders(); + } + } } public SwerveModuleState getState() { @@ -131,6 +161,8 @@ public class SwerveModule { // Probably doesn't *hurt* that it's here, but it may not be needed desiredState.optimize(new Rotation2d(turningRelativeEncoder.getPosition())); + lastTargetState = desiredState; + drive.setControl( driveVelocityRequest.withVelocity( desiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters