diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java index 1e79c15..2fbb135 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java @@ -2,19 +2,20 @@ 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, turnZeroDeg; 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[] turnZeroDeg, boolean[] driveInversion, boolean[] reversed, double driveModifier, boolean[] turnInversion) { this.wheelDiameterMeters = wheelDiameterMeters; this.driveGearing = driveGearing; - this.mu = mu;//coefficient of friction between the wheel and the surface + this.turnGearing = turnGearing; + this.mu = mu; //coefficient of friction between the wheel and the surface this.autoCentripetalAccel = autoCentripetalAccel; this.kForwardVolts = kForwardVolts; this.kForwardVels = kForwardVels; diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index d39dc47..b91aec1 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -38,6 +38,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; @@ -53,7 +59,7 @@ public enum ModuleType {FL, FR, BL, BR}; private Timer timer; private SimpleMotorFeedforward forwardSimpleMotorFF, backwardSimpleMotorFF, turnSimpleMotorFeedforward; private double maxControllableAccerlationRps2; - private double desiredSpeed, lastAngle, maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2, drivetoleranceMPerS, turnToleranceRot, angleDiffRot; + private double desiredSpeed, lastAngle, maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2, drivetoleranceMPerS, turnToleranceRot, angleDiffRot, relativePositionCorrection; private double turnSpeedCorrectionVolts, turnFFVolts, turnVolts; private double maxTurnVelocityWithoutTippingRps; @@ -73,7 +79,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().setPositionConversionFactor(positionConstant); + turn.getEncoder().setVelocityConversionFactor(positionConstant / 60); + maxControllableAccerlationRps2 = 0; final double normalForceNewtons = 83.2 /* lbf */ * 4.4482 /* N/lbf */ / 4 /* numModules */; double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; @@ -95,7 +106,6 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN drivePIDController = new PIDController(config.drivekP[arrIndex], config.drivekI[arrIndex], config.drivekD[arrIndex]); - /* offset for 1 CANcoder count */ drivetoleranceMPerS = (1.0 / (double)(drive.getEncoder().getCountsPerRevolution()) * positionConstant) / Units.millisecondsToSeconds(drive.getEncoder().getMeasurementPeriod() * drive.getEncoder().getAverageDepth()); drivePIDController.setTolerance(drivetoleranceMPerS); @@ -196,6 +206,15 @@ public void drivePeriodic() { } public void turnPeriodic() { + // 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 + double newRelativePositionCorrection = turn.getEncoder().getPosition() - getRawEncoderOrFallbackAngle(); + if(!Double.isNaN(newRelativePositionCorrection)) relativePositionCorrection = newRelativePositionCorrection; + String moduleString = type.toString(); // Turn Control { @@ -303,7 +322,14 @@ private void setAngle(double angle) { * @return module angle in degrees */ public double getModuleAngle() { - return MathUtil.inputModulus(Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValue()) - turnZeroDeg, -180, 180); + return MathUtil.inputModulus(getRawEncoderOrFallbackAngle()-turnZeroDeg, -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() ? Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValue()) : turn.getEncoder().getPosition() - relativePositionCorrection; } /** @@ -354,8 +380,12 @@ 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(moduleString + " CANCoder Connected", cancoderConnected()); + SmartDashboard.putNumber(moduleString + " Raw Encoder or Fallback Angle", getRawEncoderOrFallbackAngle()); + SmartDashboard.putNumber(moduleString + " CANCoder Last Packet", (System.nanoTime() / 1e6d) - (turnEncoder.getLastTimestamp() * 1000)); + SmartDashboard.putNumber(moduleString + " CANCoder Last Timestamp", turnEncoder.getLastTimestamp()); SmartDashboard.putNumber(moduleString + "Turn PID Output (Volts)", turnSpeedCorrectionVolts); SmartDashboard.putNumber(moduleString + "Turn FF Output (Volts)", turnFFVolts); @@ -429,6 +459,17 @@ public void setMaxTurnVelocity(double maxVel) { turnPIDController.setConstraints(turnConstraints); } + public boolean cancoderConnected() { + // 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() * 1000) < CANCODER_TIMEOUT_MS; + } + @Override public void initSendable(SendableBuilder builder) { builder.setActuator(true); @@ -449,6 +490,7 @@ 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)", () -> angleDiffRot, null); + builder.addBooleanProperty("CANCoder Connected", this::cancoderConnected, null); builder.addDoubleProperty("Turn PID Output", () -> turnSpeedCorrectionVolts, null); builder.addDoubleProperty("Turn FF Output", () -> turnFFVolts, null);