From f9d90a5648060c37096324d02010025e2f8affbd Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 30 Jun 2023 20:07:48 -0700 Subject: [PATCH 01/12] add fallback encoders to swerve --- .../lib199/swerve/SwerveConfig.java | 5 ++- .../lib199/swerve/SwerveModule.java | 37 ++++++++++++++++++- 2 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java index c7276516..2eb2f789 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java @@ -2,18 +2,19 @@ public final class SwerveConfig { - public double wheelDiameterMeters, driveGearing, mu, autoCentripetalAccel, driveModifier; + public double wheelDiameterMeters, driveGearing, turnGearing, mu, autoCentripetalAccel, driveModifier; public double[] kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZero; public boolean[] driveInversion, turnInversion, reversed; - public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu, + public SwerveConfig(double wheelDiameterMeters, double driveGearing, double turnGearing, double mu, double autoCentripetalAccel, double[] kForwardVolts, double[] kForwardVels, double[] kForwardAccels, double[] kBackwardVolts, double[] kBackwardVels, double[] kBackwardAccels, double[] drivekP, double[] drivekI, double[] drivekD, double[] turnkP, double[] turnkI, double[] turnkD, double[] turnkS, double[] turnkV, double[] turnkA, double[] turnZero, boolean[] driveInversion, boolean[] reversed, double driveModifier, boolean[] turnInversion) { this.wheelDiameterMeters = wheelDiameterMeters; this.driveGearing = driveGearing; + this.turnGearing = turnGearing; this.mu = mu; this.autoCentripetalAccel = autoCentripetalAccel; this.kForwardVolts = kForwardVolts; diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index ba85a054..80474523 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -40,7 +40,7 @@ public enum ModuleType {FL, FR, BL, BR}; private boolean reversed; private Timer timer; private SimpleMotorFeedforward forwardSimpleMotorFF, backwardSimpleMotorFF, turnSimpleMotorFeedforward; - private double desiredSpeed, lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg, angleDiff; + private double desiredSpeed, lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg, angleDiff, relativePositionCorrection; public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CANSparkMax turn, CANCoder turnEncoder, int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { @@ -56,7 +56,12 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN drive.setInverted(config.driveInversion[arrIndex]); drive.getEncoder().setPositionConversionFactor(positionConstant); drive.getEncoder().setVelocityConversionFactor(positionConstant / 60); + + positionConstant = 360 / config.turnGearing; // Convert rotations to degrees turn.setInverted(config.turnInversion[arrIndex]); + turn.getEncoder().setInverted(config.turnInversion[arrIndex]); // Check to see if this is necessary + turn.getEncoder().setPositionConversionFactor(positionConstant); + turn.getEncoder().setVelocityConversionFactor(positionConstant / 60); final double normalForceNewtons = 83.2 /* lbf */ * 4.4482 /* N/lbf */ / 4 /* numModules */; double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; @@ -78,7 +83,6 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN drivePIDController = new PIDController(2 * config.drivekP[arrIndex], config.drivekI[arrIndex], config.drivekD[arrIndex]); - //System.out.println("Velocity Constant: " + (positionConstant / 60)); @@ -127,6 +131,14 @@ public ModuleType getType() { private double prevTurnVelocity = 0; public void periodic() { + // Use a member variable instead of setPosition to reduce CAN bus traffic + // If we call the CANCoder value c, the SparkMax value s, and the correction x, + // When we're using the CANCoder, we want the correction to work out so s - x = c + // The formula here gives x = s - c => s - x = s - (s - c) = c + s - s = c as desired + // When we're using the SparkMax, we want the correction to work out so there is no chage + // The formula here gives x = s - (s - x) = x + s - s = x as desired + relativePositionCorrection = turn.getEncoder().getPosition() - getRawEncoderOrFallbackAngle(); + // Drive Control { double actualSpeed = getCurrentSpeed(); @@ -233,6 +245,13 @@ public double getModuleAngle() { return MathUtil.inputModulus(turnEncoder.getAbsolutePosition()-turnZero, -180, 180); } + /** + * @return The current angle measured by the CANCoder (not zeroed) or, if the CANCoder is disconnected, an approximated angle from the turn motor encoder. + */ + public double getRawEncoderOrFallbackAngle() { + return cancoderConnected() ? turnEncoder.getAbsolutePosition() : drive.getEncoder().getPosition() - relativePositionCorrection; + } + /** * Gets the current state (speed and angle) of this module. * @return A SwerveModuleState object representing the speed and angle of the module. @@ -282,6 +301,7 @@ public void updateSmartDashboard() { //SmartDashboard.putNumber("Gyro Roll", rollDegSupplier.get()); SmartDashboard.putNumber(moduleString + "Antigravitational Acceleration", calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get())); SmartDashboard.putBoolean(moduleString + " Turn is at Goal", turnPIDController.atGoal()); + SmartDashboard.putBoolean("CANCoder Connected", cancoderConnected()); } public void toggleMode() { @@ -304,6 +324,18 @@ public void setMaxTurnVelocity(double maxVel) { turnPIDController.setConstraints(turnConstraints); } + public boolean cancoderConnected() { + switch(turnEncoder.getMagnetFieldStrength()) { + case Good_GreenLED: + case Adequate_OrangeLED: + return true; + case BadRange_RedLED: + case Invalid_Unknown: + default: + return false; + } + } + @Override public void initSendable(SendableBuilder builder) { builder.setActuator(true); @@ -324,5 +356,6 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Error (deg)", turnPIDController::getPositionError, null); builder.addDoubleProperty("Desired Speed (mps)", drivePIDController::getSetpoint, null); builder.addDoubleProperty("Angle Diff (deg)", () -> angleDiff, null); + builder.addBooleanProperty("CANCoder Connected", this::cancoderConnected, null); } } From 14fdad27e2cb4fe5263282ee437108b4758bb915 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 30 Jun 2023 20:16:25 -0700 Subject: [PATCH 02/12] remove turn encoder inversion (not necessary for brushless) --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 80474523..e8d5ccac 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -59,7 +59,6 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN positionConstant = 360 / config.turnGearing; // Convert rotations to degrees turn.setInverted(config.turnInversion[arrIndex]); - turn.getEncoder().setInverted(config.turnInversion[arrIndex]); // Check to see if this is necessary turn.getEncoder().setPositionConversionFactor(positionConstant); turn.getEncoder().setVelocityConversionFactor(positionConstant / 60); From 7125c22724c998ddc780691eb069672c01968cd2 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 30 Jun 2023 20:35:10 -0700 Subject: [PATCH 03/12] fix swervemodule telemetry --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index e8d5ccac..df7481cb 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -298,9 +298,9 @@ public void updateSmartDashboard() { SmartDashboard.putNumber(moduleString + " Turn Goal Vel (dps)", turnPIDController.getGoal().velocity); //SmartDashboard.putNumber("Gyro Pitch", pitchDegSupplier.get()); //SmartDashboard.putNumber("Gyro Roll", rollDegSupplier.get()); - SmartDashboard.putNumber(moduleString + "Antigravitational Acceleration", calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get())); + SmartDashboard.putNumber(moduleString + " Antigravitational Acceleration", calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get())); SmartDashboard.putBoolean(moduleString + " Turn is at Goal", turnPIDController.atGoal()); - SmartDashboard.putBoolean("CANCoder Connected", cancoderConnected()); + SmartDashboard.putBoolean(moduleString + " CANCoder Connected", cancoderConnected()); } public void toggleMode() { From 7bd5a02405cdc001faadff3f2c96758a00d611b5 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 30 Jun 2023 20:44:09 -0700 Subject: [PATCH 04/12] more telemetry --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index df7481cb..7977cff1 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -301,6 +301,7 @@ public void updateSmartDashboard() { SmartDashboard.putNumber(moduleString + " Antigravitational Acceleration", calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get())); SmartDashboard.putBoolean(moduleString + " Turn is at Goal", turnPIDController.atGoal()); SmartDashboard.putBoolean(moduleString + " CANCoder Connected", cancoderConnected()); + SmartDashboard.putNumber(moduleString + " Raw Encoder or Fallback Angle", getRawEncoderOrFallbackAngle()); } public void toggleMode() { From cce32a6bcce3038394e06ca41edee61713e8c1fc Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Tue, 4 Jul 2023 18:52:43 -0700 Subject: [PATCH 05/12] use monolithic clock for can packet timings --- .../lib199/swerve/SwerveModule.java | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 7977cff1..b5eff252 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -26,6 +26,12 @@ * such as moving, getting encoder values, or configuring PID. */ public class SwerveModule implements Sendable { + + /** + * If a CANCoder has not sent a packet in this amount of time, it is considered disconnected. + */ + public static final double CANCODER_TIMEOUT_MS = 200; + public enum ModuleType {FL, FR, BL, BR}; private SwerveConfig config; @@ -325,15 +331,14 @@ public void setMaxTurnVelocity(double maxVel) { } public boolean cancoderConnected() { - switch(turnEncoder.getMagnetFieldStrength()) { - case Good_GreenLED: - case Adequate_OrangeLED: - return true; - case BadRange_RedLED: - case Invalid_Unknown: - default: - return false; - } + // The right thing to do here would be to have WPILib tell us this time, but that functionality + // is not exposed in the 2023 API. + // From what I can tell, WPILib bases CAN packet timestamps off of the system's monotonic clock + // On linux (and maybe other OSs), this is exposed through System.nanoTime() + // In WPILib 2024, this should be replaced with CAN.getCANPacketBaseTime() (see wpilibsuite/allwpilib#5357) + // That function should also return millis rather than nanos, eliminating the need + // for the below conversion. + return (System.nanoTime() / 1e6d) - turnEncoder.getLastTimestamp() < CANCODER_TIMEOUT_MS; } @Override From 8265d492e81945809eec4adbe1900fc15b504f77 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 7 Jul 2023 18:32:26 -0700 Subject: [PATCH 06/12] put lasttimestamp to smartdashboard --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index b5eff252..a6b5cf94 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -308,6 +308,7 @@ public void updateSmartDashboard() { SmartDashboard.putBoolean(moduleString + " Turn is at Goal", turnPIDController.atGoal()); SmartDashboard.putBoolean(moduleString + " CANCoder Connected", cancoderConnected()); SmartDashboard.putNumber(moduleString + " Raw Encoder or Fallback Angle", getRawEncoderOrFallbackAngle()); + SmartDashboard.putNumber(moduleString + " CANCoder Last Packet", (System.nanoTime() / 1e6d) - turnEncoder.getLastTimestamp()); } public void toggleMode() { From 3456e7738c44bc35c09c6b68f5f9752c4177ef2c Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 7 Jul 2023 18:37:46 -0700 Subject: [PATCH 07/12] print last timestamp --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index a6b5cf94..31a57035 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -309,6 +309,7 @@ public void updateSmartDashboard() { SmartDashboard.putBoolean(moduleString + " CANCoder Connected", cancoderConnected()); SmartDashboard.putNumber(moduleString + " Raw Encoder or Fallback Angle", getRawEncoderOrFallbackAngle()); SmartDashboard.putNumber(moduleString + " CANCoder Last Packet", (System.nanoTime() / 1e6d) - turnEncoder.getLastTimestamp()); + SmartDashboard.putNumber(moduleString + " CANCoder Last Timestamp", turnEncoder.getLastTimestamp()); } public void toggleMode() { From 8e722e89cd8c33a8ab9c2a12bb83fb3c4247673e Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 7 Jul 2023 18:43:13 -0700 Subject: [PATCH 08/12] assume getLastTimestamp is in seconds --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 31a57035..aa82c414 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -308,7 +308,7 @@ public void updateSmartDashboard() { SmartDashboard.putBoolean(moduleString + " Turn is at Goal", turnPIDController.atGoal()); SmartDashboard.putBoolean(moduleString + " CANCoder Connected", cancoderConnected()); SmartDashboard.putNumber(moduleString + " Raw Encoder or Fallback Angle", getRawEncoderOrFallbackAngle()); - SmartDashboard.putNumber(moduleString + " CANCoder Last Packet", (System.nanoTime() / 1e6d) - turnEncoder.getLastTimestamp()); + SmartDashboard.putNumber(moduleString + " CANCoder Last Packet", (System.nanoTime() / 1e6d) - (turnEncoder.getLastTimestamp() * 1000)); SmartDashboard.putNumber(moduleString + " CANCoder Last Timestamp", turnEncoder.getLastTimestamp()); } @@ -340,7 +340,7 @@ public boolean cancoderConnected() { // In WPILib 2024, this should be replaced with CAN.getCANPacketBaseTime() (see wpilibsuite/allwpilib#5357) // That function should also return millis rather than nanos, eliminating the need // for the below conversion. - return (System.nanoTime() / 1e6d) - turnEncoder.getLastTimestamp() < CANCODER_TIMEOUT_MS; + return (System.nanoTime() / 1e6d) - (turnEncoder.getLastTimestamp() * 1000) < CANCODER_TIMEOUT_MS; } @Override From f09551263461957de3f9ff6ae114de433b8e8160 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 7 Jul 2023 18:58:39 -0700 Subject: [PATCH 09/12] use correct encoder --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index aa82c414..bf015e0c 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -254,7 +254,7 @@ public double getModuleAngle() { * @return The current angle measured by the CANCoder (not zeroed) or, if the CANCoder is disconnected, an approximated angle from the turn motor encoder. */ public double getRawEncoderOrFallbackAngle() { - return cancoderConnected() ? turnEncoder.getAbsolutePosition() : drive.getEncoder().getPosition() - relativePositionCorrection; + return cancoderConnected() ? turnEncoder.getAbsolutePosition() : turn.getEncoder().getPosition() - relativePositionCorrection; } /** From b88e36c8ba3c99cb778e8f6f17e3207d63985af1 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 7 Jul 2023 19:11:55 -0700 Subject: [PATCH 10/12] fix getModuleAngle --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index bf015e0c..a316fcd8 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -247,7 +247,7 @@ private void setAngle(double angle) { * @return module angle in degrees */ public double getModuleAngle() { - return MathUtil.inputModulus(turnEncoder.getAbsolutePosition()-turnZero, -180, 180); + return MathUtil.inputModulus(getRawEncoderOrFallbackAngle()-turnZero, -180, 180); } /** From 902eff5ad6a7fb7663683123ec520c09ab3a2e8f Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 7 Jul 2023 19:50:06 -0700 Subject: [PATCH 11/12] prevent nan values --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index a316fcd8..e025fd26 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -142,7 +142,8 @@ public void periodic() { // The formula here gives x = s - c => s - x = s - (s - c) = c + s - s = c as desired // When we're using the SparkMax, we want the correction to work out so there is no chage // The formula here gives x = s - (s - x) = x + s - s = x as desired - relativePositionCorrection = turn.getEncoder().getPosition() - getRawEncoderOrFallbackAngle(); + double newRelativePositionCorrection = turn.getEncoder().getPosition() - getRawEncoderOrFallbackAngle(); + if(!Double.isNaN(newRelativePositionCorrection)) relativePositionCorrection = newRelativePositionCorrection; // Drive Control { From e74dd79ec25490a946f6f1d225568cbc848d38b1 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 7 Jul 2023 19:52:45 -0700 Subject: [PATCH 12/12] empty commit