diff --git a/build.gradle b/build.gradle index b9c88c2..334cca4 100644 --- a/build.gradle +++ b/build.gradle @@ -15,7 +15,7 @@ java { } // set a valid semver version -version = '4.1.2' +version = '4.1.3-test1' repositories { mavenCentral() diff --git a/src/main/java/ca/team1310/swerve/SwerveTelemetry.java b/src/main/java/ca/team1310/swerve/SwerveTelemetry.java index ff89ae7..7017bbd 100644 --- a/src/main/java/ca/team1310/swerve/SwerveTelemetry.java +++ b/src/main/java/ca/team1310/swerve/SwerveTelemetry.java @@ -78,6 +78,12 @@ public final class SwerveTelemetry { /** The drive motor output power of the swerve modules */ public double[] driveMotorOutputPower; + /** + * Per-module angle error in degrees: difference between relative encoder angle and absolute + * encoder angle. Useful for monitoring encoder drift. + */ + public double[] moduleAngleErrorDegrees; + // Pose /** The x location of the robot with respect to the field in metres */ public double poseMetresX = Double.MIN_VALUE; @@ -145,6 +151,7 @@ public SwerveTelemetry(int moduleCount) { moduleAngleMotorPositionDegrees = new double[moduleCount]; moduleDriveMotorPositionMetres = new double[moduleCount]; driveMotorOutputPower = new double[moduleCount]; + moduleAngleErrorDegrees = new double[moduleCount]; } /** Post all telemetry data to SmartDashboard */ @@ -221,5 +228,7 @@ private void postVerbose() { double pwr = driveMotorOutputPower[i]; SmartDashboard.putString(PREFIX + "Swerve/drive_power_" + name, String.format("%.3f", pwr)); } + + SmartDashboard.putNumberArray(PREFIX + "Swerve/angleErrors", moduleAngleErrorDegrees); } } diff --git a/src/main/java/ca/team1310/swerve/core/AbsoluteAngleEncoder.java b/src/main/java/ca/team1310/swerve/core/AbsoluteAngleEncoder.java index 73d821a..591ae49 100644 --- a/src/main/java/ca/team1310/swerve/core/AbsoluteAngleEncoder.java +++ b/src/main/java/ca/team1310/swerve/core/AbsoluteAngleEncoder.java @@ -12,6 +12,20 @@ public interface AbsoluteAngleEncoder { */ double getPosition(); + /** + * Get the cached absolute location of the encoder without performing expensive refresh or health + * check operations. The cached value comes from the StatusSignal's auto-refresh (typically + * 100Hz), so it is at most 10ms old. + * + *

This is suitable for high-frequency reads such as odometry updates where latency of a full + * refresh is unacceptable. + * + * @return The absolute location of the encoder in degrees, from 0 to 360. Returns -1 on error. + */ + default double getPositionCached() { + return getPosition(); + } + /** * Are there any active faults on this motor * diff --git a/src/main/java/ca/team1310/swerve/core/AngleMotor.java b/src/main/java/ca/team1310/swerve/core/AngleMotor.java index f3dbb0e..4ea03c2 100644 --- a/src/main/java/ca/team1310/swerve/core/AngleMotor.java +++ b/src/main/java/ca/team1310/swerve/core/AngleMotor.java @@ -34,6 +34,20 @@ public interface AngleMotor { */ void setEncoderPosition(double actualAngleDegrees); + /** + * Gradually correct the internal encoder position toward the absolute angle. Unlike {@link + * #setEncoderPosition(double)}, this applies a fractional correction to avoid discontinuities in + * the PID feedback signal during active steering. + * + *

The default implementation delegates to {@link #setEncoderPosition(double)}. + * + * @param absoluteAngleDegrees the true angle from the absolute encoder (0 to 360) + * @param correctionFactor fraction of the error to apply per call (e.g. 0.2 = 20%) + */ + default void correctEncoderPosition(double absoluteAngleDegrees, double correctionFactor) { + setEncoderPosition(absoluteAngleDegrees); + } + /** * Are there any active faults on this motor * diff --git a/src/main/java/ca/team1310/swerve/core/CoreSwerveDrive.java b/src/main/java/ca/team1310/swerve/core/CoreSwerveDrive.java index 9cb323d..ec722c3 100644 --- a/src/main/java/ca/team1310/swerve/core/CoreSwerveDrive.java +++ b/src/main/java/ca/team1310/swerve/core/CoreSwerveDrive.java @@ -5,6 +5,7 @@ import ca.team1310.swerve.RunnymedeSwerveDrive; import ca.team1310.swerve.SwerveTelemetry; import ca.team1310.swerve.core.config.CoreSwerveConfig; +import ca.team1310.swerve.utils.SwerveUtils; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotBase; @@ -50,6 +51,9 @@ public class CoreSwerveDrive implements RunnymedeSwerveDrive { private final Notifier moduleManagementThread = new Notifier(this::updateModules); + private static final int SYNC_ENCODERS_PERIOD_MS = 500; + private final Notifier encoderSyncThread = new Notifier(this::syncAllEncoders); + public static final int TELEMETRY_UPDATE_PERIOD_MS = 50; // milliseconds private final Notifier telemetryThread = new Notifier(this::updateTelemetry); @@ -61,6 +65,7 @@ public class CoreSwerveDrive implements RunnymedeSwerveDrive { protected CoreSwerveDrive(CoreSwerveConfig cfg) { System.out.println("Initializing RunnymedeSwerve."); System.out.println("Swerve module update period: " + MANAGE_MODULES_PERIOD_MS + " ms"); + System.out.println("Swerve encoder sync period: " + SYNC_ENCODERS_PERIOD_MS + " ms"); System.out.println("Swerve telemetry update period: " + TELEMETRY_UPDATE_PERIOD_MS + " ms"); // order matters in case we want to use AdvantageScope @@ -121,6 +126,9 @@ protected CoreSwerveDrive(CoreSwerveConfig cfg) { moduleManagementThread.setName("RunnymedeSwerve manageModuleStates"); moduleManagementThread.startPeriodic(MANAGE_MODULES_PERIOD_MS / 1000.0); + encoderSyncThread.setName("RunnymedeSwerve syncEncoders"); + encoderSyncThread.startPeriodic(SYNC_ENCODERS_PERIOD_MS / 1000.0); + telemetryThread.setName("RunnymedeSwerve updateTelemetry"); // in simulation mode, provide telemetry faster but while driving use slower rate telemetryThread.startPeriodic(isSimulation ? .02 : TELEMETRY_UPDATE_PERIOD_MS / 1000.0); @@ -161,6 +169,13 @@ protected synchronized void updateModules() { } } + /** Sync relative encoders toward absolute encoders on a separate, slower thread. */ + private synchronized void syncAllEncoders() { + for (SwerveModule module : modules) { + module.syncEncoders(); + } + } + /** Update the gyro in case the robot is running in simulation mode. */ protected void updateGyroForSimulation() {} @@ -293,6 +308,14 @@ protected synchronized void updateTelemetry(SwerveTelemetry telemetry) { telemetry.driveMotorOutputPower[i] = state.getDriveOutputPower(); // angle encoder telemetry.moduleAbsoluteEncoderPositionDegrees[i] = state.getAbsoluteEncoderAngle(); + + // Compute per-module angle error (relative vs absolute encoder) + double absAngle = state.getAbsoluteEncoderAngle(); + if (absAngle >= 0) { + double absNormalized = SwerveUtils.normalizeDegrees(absAngle); + double error = SwerveUtils.normalizeDegrees(state.getAngle() - absNormalized); + telemetry.moduleAngleErrorDegrees[i] = error; + } } } // post it! diff --git a/src/main/java/ca/team1310/swerve/core/ModuleState.java b/src/main/java/ca/team1310/swerve/core/ModuleState.java index 814b42e..25d19d2 100644 --- a/src/main/java/ca/team1310/swerve/core/ModuleState.java +++ b/src/main/java/ca/team1310/swerve/core/ModuleState.java @@ -1,5 +1,7 @@ package ca.team1310.swerve.core; +import static ca.team1310.swerve.utils.SwerveUtils.normalizeDegrees; + import ca.team1310.swerve.utils.Coordinates; /** @@ -89,6 +91,20 @@ public double getAngle() { return anglePosition; } + /** + * Get the best available angle for odometry. Returns the absolute encoder angle (normalized to + * -180..180) if available, falling back to the relative encoder angle. The absolute encoder is + * preferred because the relative encoder can drift between sync cycles. + * + * @return the angle in degrees from -180 to 180 (ccw positive) + */ + public double getOdometryAngle() { + if (absoluteEncoderAngle >= 0) { + return normalizeDegrees(absoluteEncoderAngle); + } + return anglePosition; + } + /** * Get the speed of the module * diff --git a/src/main/java/ca/team1310/swerve/core/SwerveModule.java b/src/main/java/ca/team1310/swerve/core/SwerveModule.java index 661e1b2..8730714 100644 --- a/src/main/java/ca/team1310/swerve/core/SwerveModule.java +++ b/src/main/java/ca/team1310/swerve/core/SwerveModule.java @@ -37,6 +37,12 @@ public interface SwerveModule { */ void readState(); + /** + * Synchronize the relative encoder toward the absolute encoder. Called every module update cycle + * after {@link #readState()}. + */ + void syncEncoders(); + /** * Update the internal state of the swerve module's detailed telemetry information. It does NOT * include data loaded by readState(). Some of these operations can be slow and should therefore diff --git a/src/main/java/ca/team1310/swerve/core/SwerveModuleImpl.java b/src/main/java/ca/team1310/swerve/core/SwerveModuleImpl.java index 60a52c7..e79eb4a 100644 --- a/src/main/java/ca/team1310/swerve/core/SwerveModuleImpl.java +++ b/src/main/java/ca/team1310/swerve/core/SwerveModuleImpl.java @@ -9,12 +9,13 @@ import ca.team1310.swerve.math.SwerveMath; import ca.team1310.swerve.utils.Coordinates; import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Notifier; class SwerveModuleImpl implements SwerveModule { - private static final double ANGLE_ENCODER_SYNC_PERIOD_MS = 500; - private final Notifier encoderSynchronizer = new Notifier(this::syncAngleEncoder); + private static final boolean USE_GRADUAL_ENCODER_CORRECTION = true; + private static final double ENCODER_CORRECTION_FACTOR = 0.1; + private static final int HARD_SYNC_CYCLE_INTERVAL = + 500 / CoreSwerveDrive.MANAGE_MODULES_PERIOD_MS; // ~50 cycles = 500ms private final String name; private final Coordinates location; @@ -23,6 +24,7 @@ class SwerveModuleImpl implements SwerveModule { private final AbsoluteAngleEncoder angleEncoder; private ModuleDirective desiredState = new ModuleDirective(); private final ModuleState measuredState = new ModuleState(); + private int hardSyncCycleCounter = 0; private final Alert driveMotorFaultPresent; private final Alert angleMotorFaultPresent; @@ -30,21 +32,12 @@ class SwerveModuleImpl implements SwerveModule { SwerveModuleImpl(ModuleConfig cfg, double maxAttainableModuleSpeedMps) { this.name = cfg.name(); - System.out.println( - "Swerve (" - + this.name - + ") absolute angle encoder sync period: " - + ANGLE_ENCODER_SYNC_PERIOD_MS - + " ms"); this.location = cfg.location(); measuredState.setLocation(cfg.location()); this.driveMotor = getDriveMotor(cfg, maxAttainableModuleSpeedMps); this.angleMotor = getAngleMotor(cfg); this.angleEncoder = getAbsoluteAngleEncoder(cfg); - this.encoderSynchronizer.setName("RunnymedeSwerve Angle Encoder Sync " + name); - this.encoderSynchronizer.startPeriodic(ANGLE_ENCODER_SYNC_PERIOD_MS / 1000); - driveMotorFaultPresent = new Alert("Swerve Drive Motor [" + name + "] Fault Present", Alert.AlertType.kError); angleMotorFaultPresent = @@ -89,10 +82,6 @@ private AbsoluteAngleEncoder getAbsoluteAngleEncoder(ModuleConfig cfg) { cfg.absoluteAngleEncoderConfig()); } - private synchronized void syncAngleEncoder() { - angleMotor.setEncoderPosition(angleEncoder.getPosition()); - } - public String getName() { return name; } @@ -103,6 +92,20 @@ public synchronized void readState() { measuredState.setAngle(angleMotor.getPosition()); measuredState.setPosition(driveMotor.getDistance()); + measuredState.setAbsoluteEncoderAngle(angleEncoder.getPositionCached()); + } + + public synchronized void syncEncoders() { + double absoluteEncoderAngle = measuredState.getAbsoluteEncoderAngle(); + if (absoluteEncoderAngle < 0) { + return; // invalid CANCoder reading (-1 means error) + } + if (USE_GRADUAL_ENCODER_CORRECTION) { + angleMotor.correctEncoderPosition(absoluteEncoderAngle, ENCODER_CORRECTION_FACTOR); + } else if (++hardSyncCycleCounter >= HARD_SYNC_CYCLE_INTERVAL) { + hardSyncCycleCounter = 0; + angleMotor.setEncoderPosition(absoluteEncoderAngle); + } } public synchronized void readVerboseState() { @@ -118,7 +121,7 @@ public synchronized ModuleState getState() { public synchronized void setDesiredState(ModuleDirective desiredState) { this.desiredState = desiredState; - double currentHeadingDeg = angleMotor.getPosition(); + double currentHeadingDeg = measuredState.getOdometryAngle(); SwerveMath.optimizeWheelAngles(desiredState, currentHeadingDeg); SwerveMath.cosineCompensator(desiredState, currentHeadingDeg); diff --git a/src/main/java/ca/team1310/swerve/core/SwerveModuleSimulation.java b/src/main/java/ca/team1310/swerve/core/SwerveModuleSimulation.java index 0279a52..ccf9323 100644 --- a/src/main/java/ca/team1310/swerve/core/SwerveModuleSimulation.java +++ b/src/main/java/ca/team1310/swerve/core/SwerveModuleSimulation.java @@ -38,6 +38,9 @@ public Coordinates getLocation() { @Override public void readState() {} + @Override + public void syncEncoders() {} + @Override public void readVerboseState() {} diff --git a/src/main/java/ca/team1310/swerve/core/hardware/cancoder/CanCoder.java b/src/main/java/ca/team1310/swerve/core/hardware/cancoder/CanCoder.java index 4327c7b..7db527f 100644 --- a/src/main/java/ca/team1310/swerve/core/hardware/cancoder/CanCoder.java +++ b/src/main/java/ca/team1310/swerve/core/hardware/cancoder/CanCoder.java @@ -100,6 +100,14 @@ public double getPosition() { return measuredPosition; } + @Override + public double getPositionCached() { + if (angle.getStatus() != StatusCode.OK) { + return -1; + } + return ((angle.getValue().in(Degrees) - absoluteEncoderOffset) + 360) % 360; + } + private double calculatePosition() { MagnetHealthValue strength = magnetHealth.refresh().getValue(); diff --git a/src/main/java/ca/team1310/swerve/core/hardware/rev/neospark/NSAngleMotor.java b/src/main/java/ca/team1310/swerve/core/hardware/rev/neospark/NSAngleMotor.java index 9cb71c2..53b83a5 100644 --- a/src/main/java/ca/team1310/swerve/core/hardware/rev/neospark/NSAngleMotor.java +++ b/src/main/java/ca/team1310/swerve/core/hardware/rev/neospark/NSAngleMotor.java @@ -119,6 +119,17 @@ public void setReferenceAngle(double degrees) { doWithRetry(() -> controller.setReference(degrees, SparkBase.ControlType.kPosition)); } + @Override + public void correctEncoderPosition(double absoluteAngleDegrees, double correctionFactor) { + double rawPosition = encoder.getPosition(); + double normalizedPosition = normalizeDegrees(rawPosition); + double error = normalizeDegrees(absoluteAngleDegrees - normalizedPosition); + if (Math.abs(error) < 0.1) { + return; // dead band: avoid CAN traffic for negligible corrections + } + encoder.setPosition(rawPosition + correctionFactor * error); // fire-and-forget + } + @Override public void setEncoderPosition(double actualAngleDegrees) { double omega = Math.abs(encoder.getVelocity()); diff --git a/src/main/java/ca/team1310/swerve/odometry/FieldAwareSwerveDrive.java b/src/main/java/ca/team1310/swerve/odometry/FieldAwareSwerveDrive.java index 57e4d24..12cdb13 100644 --- a/src/main/java/ca/team1310/swerve/odometry/FieldAwareSwerveDrive.java +++ b/src/main/java/ca/team1310/swerve/odometry/FieldAwareSwerveDrive.java @@ -106,7 +106,7 @@ private synchronized SwerveModulePosition[] getSwerveModulePositions() { var states = getModuleStates(); for (int i = 0; i < states.length; i++) { modulePosition[i].distanceMeters = states[i].getPosition(); - modulePosition[i].angle = Rotation2d.fromDegrees(states[i].getAngle()); + modulePosition[i].angle = Rotation2d.fromDegrees(states[i].getOdometryAngle()); } return modulePosition; } diff --git a/src/main/java/ca/team1310/swerve/vision/LimelightAwareSwerveDrive.java b/src/main/java/ca/team1310/swerve/vision/LimelightAwareSwerveDrive.java index 3f59993..52834b9 100644 --- a/src/main/java/ca/team1310/swerve/vision/LimelightAwareSwerveDrive.java +++ b/src/main/java/ca/team1310/swerve/vision/LimelightAwareSwerveDrive.java @@ -25,9 +25,8 @@ public class LimelightAwareSwerveDrive extends FieldAwareSwerveDrive { private static final int OFFSET_POSE_Y = 1; private static final int OFFSET_POSE_ROTATION_YAW = 5; private static final int OFFSET_TOTAL_LATENCY = 6; - - // Standard Deviations Used for most of the Pose Updates - private static final Matrix MEGATAG2_STDDEV = VecBuilder.fill(0.06, 0.06, 9999999); + private static final int OFFSET_TAG_COUNT = 7; + private static final int OFFSET_AVG_TAG_DISTANCE = 9; // Orientation publishers private final DoubleArrayPublisher llRobotOrientation; @@ -40,6 +39,14 @@ public class LimelightAwareSwerveDrive extends FieldAwareSwerveDrive { private final double fieldExtentX; private final double fieldExtentY; + // Vision trust parameters + private final double baseStdDevXY; + private final double baseStdDevTheta; + private final double distanceScaleFactor; + private final double maxTrustDistanceMetres; + private final double outlierRejectionThresholdMetres; + private final int minTagCountForLowStdDev; + // Data for telemetry publishing private Pose2d visPose = null; @@ -57,6 +64,12 @@ public LimelightAwareSwerveDrive( this.fieldExtentX = limelightConfig.fieldExtentX(); this.fieldExtentY = limelightConfig.fieldExtentY(); + this.baseStdDevXY = limelightConfig.baseStdDevXY(); + this.baseStdDevTheta = limelightConfig.baseStdDevTheta(); + this.distanceScaleFactor = limelightConfig.distanceScaleFactor(); + this.maxTrustDistanceMetres = limelightConfig.maxTrustDistanceMetres(); + this.outlierRejectionThresholdMetres = limelightConfig.outlierRejectionThresholdMetres(); + this.minTagCountForLowStdDev = limelightConfig.minTagCountForLowStdDev(); final NetworkTable limelightNT = NetworkTableInstance.getDefault().getTable("limelight-" + limelightConfig.limelightName()); @@ -66,6 +79,29 @@ public LimelightAwareSwerveDrive( llMegaTag2 = limelightNT.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[0]); } + /** + * Compute dynamic vision standard deviations based on tag count and average tag distance. + * + * @param tagCount number of tags visible + * @param avgTagDistance average distance to visible tags in metres + * @return standard deviation matrix, or null if the measurement should be rejected + */ + private Matrix computeVisionStdDevs(int tagCount, double avgTagDistance) { + // Reject readings beyond max trust distance + if (avgTagDistance > maxTrustDistanceMetres) { + return null; + } + + // Tag count penalty: single tags are less reliable + double tagPenalty = tagCount < minTagCountForLowStdDev ? 2.0 : 1.0; + + // Formula: stddev = baseStdDev * (1 + distanceScaleFactor * distance^2) * tagPenalty + double xyStdDev = + baseStdDevXY * (1.0 + distanceScaleFactor * avgTagDistance * avgTagDistance) * tagPenalty; + + return VecBuilder.fill(xyStdDev, xyStdDev, baseStdDevTheta); + } + @Override protected final synchronized void updateOdometry() { super.updateOdometry(); @@ -86,6 +122,7 @@ protected final synchronized void updateOdometry() { // Get the MegaTag data - can be multiple readings since we last checked Pose2d newVisPose = null; + Pose2d currentPose = getPose(); for (var megaTagAtomic : llMegaTag2.readQueue()) { double[] megaTagData = megaTagAtomic.value; @@ -101,6 +138,8 @@ protected final synchronized void updateOdometry() { megaTagData[OFFSET_POSE_Y], Rotation2d.fromDegrees(megaTagData[OFFSET_POSE_ROTATION_YAW])); double totalLatencyMillis = megaTagData[OFFSET_TOTAL_LATENCY]; + int tagCount = (int) megaTagData[OFFSET_TAG_COUNT]; + double avgTagDistance = megaTagData[OFFSET_AVG_TAG_DISTANCE]; // Ensure pose is on field if (botPose.getX() > 0 @@ -108,9 +147,21 @@ protected final synchronized void updateOdometry() { && botPose.getX() < fieldExtentX && botPose.getY() < fieldExtentY) { + // Outlier rejection: skip poses too far from current estimate + if (currentPose.getTranslation().getDistance(botPose.getTranslation()) + > outlierRejectionThresholdMetres) { + continue; + } + + // Compute dynamic standard deviations + Matrix stdDevs = computeVisionStdDevs(tagCount, avgTagDistance); + if (stdDevs == null) { + continue; + } + // Good data, let's update the pose estimator double latencySeconds = (timestampMicros / 1000000.0) - (totalLatencyMillis / 1000.0); - getPoseEstimator().addVisionMeasurement(botPose, latencySeconds, MEGATAG2_STDDEV); + getPoseEstimator().addVisionMeasurement(botPose, latencySeconds, stdDevs); newVisPose = botPose; } } diff --git a/src/main/java/ca/team1310/swerve/vision/config/LimelightConfig.java b/src/main/java/ca/team1310/swerve/vision/config/LimelightConfig.java index 8abdf72..47fa1be 100644 --- a/src/main/java/ca/team1310/swerve/vision/config/LimelightConfig.java +++ b/src/main/java/ca/team1310/swerve/vision/config/LimelightConfig.java @@ -4,10 +4,42 @@ package ca.team1310.swerve.vision.config; /** + * Configuration for a Limelight vision system, including vision trust parameters for pose + * estimation fusion. + * * @param limelightName the name of the limelight to use * @param fieldExtentX the extent of the field in the X direction * @param fieldExtentY the extent of the field in the Y direction + * @param baseStdDevXY base standard deviation for vision XY measurements in metres + * @param baseStdDevTheta base standard deviation for vision heading measurements in radians + * @param distanceScaleFactor scaling factor for std dev growth with distance squared + * @param maxTrustDistanceMetres reject vision readings beyond this average tag distance + * @param outlierRejectionThresholdMetres reject vision poses farther than this from current + * estimate + * @param minTagCountForLowStdDev minimum tag count to get the best (1x) std dev; fewer tags get a + * 2x penalty * @author Tony Field * @since 2025-09-23 16:16 */ -public record LimelightConfig(String limelightName, double fieldExtentX, double fieldExtentY) {} +public record LimelightConfig( + String limelightName, + double fieldExtentX, + double fieldExtentY, + double baseStdDevXY, + double baseStdDevTheta, + double distanceScaleFactor, + double maxTrustDistanceMetres, + double outlierRejectionThresholdMetres, + int minTagCountForLowStdDev) { + + /** + * Backward-compatible constructor with default vision trust parameters. + * + * @param limelightName the name of the limelight to use + * @param fieldExtentX the extent of the field in the X direction + * @param fieldExtentY the extent of the field in the Y direction + */ + public LimelightConfig(String limelightName, double fieldExtentX, double fieldExtentY) { + this(limelightName, fieldExtentX, fieldExtentY, 0.4, 9999999, 0.08, 5.0, 1.0, 2); + } +}