From d72200371b39b08c69ba800646816d1667b46578 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 4 May 2024 14:09:47 -0400 Subject: [PATCH] refactor: Rewrite configs using the builder pattern. --- .../frc/lib/config/AbsoluteEncoderConfig.java | 119 +++++----- .../lib/config/FeedbackControllerConfig.java | 203 +++++++--------- .../config/FeedforwardControllerConfig.java | 139 +++++------ .../java/frc/lib/config/MechanismConfig.java | 221 +++++++++--------- .../frc/lib/config/MotionProfileConfig.java | 85 ++++--- src/main/java/frc/lib/config/MotorConfig.java | 157 ++++++------- src/main/java/frc/robot/arm/Arm.java | 54 ++--- src/main/java/frc/robot/intake/Intake.java | 67 +++--- src/main/java/frc/robot/shooter/Shooter.java | 68 +++--- src/main/java/frc/robot/swerve/Swerve.java | 69 +++--- .../java/frc/robot/swerve/SwerveFactory.java | 35 +-- 11 files changed, 533 insertions(+), 684 deletions(-) diff --git a/src/main/java/frc/lib/config/AbsoluteEncoderConfig.java b/src/main/java/frc/lib/config/AbsoluteEncoderConfig.java index 4ccd1b6..9c2ede7 100644 --- a/src/main/java/frc/lib/config/AbsoluteEncoderConfig.java +++ b/src/main/java/frc/lib/config/AbsoluteEncoderConfig.java @@ -4,77 +4,64 @@ import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Objects; /** Absolute encoder config. */ -public class AbsoluteEncoderConfig { +public record AbsoluteEncoderConfig( + boolean ccwPositive, double sensorToMechanismRatio, Rotation2d offset) { - /** Interpret counterclockwise rotation on the encoder as having positive velocity. */ - private boolean ccwPositive = true; - - /** Ratio between the absolute encoder and the mechanism. */ - private double sensorToMechanismRatio = 1.0; - - /** Offset between absolute encoder reading and mechanism position. */ - private Rotation2d offset = new Rotation2d(); - - /** - * Modifies this absolute encoder config's counterclockwise positive. - * - * @param ccwPositive the counterclockwise positive. - * @return this absolute encoder config. - */ - public AbsoluteEncoderConfig withCCWPositive(boolean ccwPositive) { - this.ccwPositive = ccwPositive; - return this; + public AbsoluteEncoderConfig { + Objects.requireNonNull(ccwPositive); + Objects.requireNonNull(sensorToMechanismRatio); + Objects.requireNonNull(offset); } - /** - * Modifies this absolute encoder config's sensor to mechanism ratio. - * - * @param sensorToMechanismRatio the sensor to mechanism ratio. - * @return this absolute encoder config. - */ - public AbsoluteEncoderConfig withSensorToMechanismRatio(double sensorToMechanismRatio) { - this.sensorToMechanismRatio = sensorToMechanismRatio; - return this; - } - - /** - * Modifies this absolute encoder config's offset. - * - * @param offset the offset. - * @return this absolute encoder config. - */ - public AbsoluteEncoderConfig withOffset(Rotation2d offset) { - this.offset = offset; - return this; - } - - /** - * Returns true if the absolute encoder is counterclockwise positive. - * - * @return true if the absolute encoder is counterclockwise positive. - */ - public boolean ccwPositive() { - return ccwPositive; - } - - /** - * Returns the absolute encoder sensor to mechanism ratio. - * - * @return the absolute encoder sensor to mechanism ratio. - */ - public double sensorToMechanismRatio() { - return sensorToMechanismRatio; - } - - /** - * Returns the absolute encoder offset. - * - * @return the absolute encoder offset. - */ - public Rotation2d offset() { - return offset; + public static final class AbsoluteEncoderConfigBuilder { + /** Interpret counterclockwise rotation on the encoder as having positive velocity. */ + private boolean ccwPositive; + + /** Ratio between the absolute encoder and the mechanism. */ + private double sensorToMechanismRatio; + + /** Offset between absolute encoder reading and mechanism position. */ + private Rotation2d offset; + + public static AbsoluteEncoderConfigBuilder defaults() { + return new AbsoluteEncoderConfigBuilder(true, 1.0, new Rotation2d()); + } + + public static AbsoluteEncoderConfigBuilder from(AbsoluteEncoderConfig absoluteEncoderConfig) { + return new AbsoluteEncoderConfigBuilder( + absoluteEncoderConfig.ccwPositive, + absoluteEncoderConfig.sensorToMechanismRatio, + absoluteEncoderConfig.offset); + } + + private AbsoluteEncoderConfigBuilder( + boolean ccwPositive, double sensorToMechanismRatio, Rotation2d offset) { + this.ccwPositive = ccwPositive; + this.sensorToMechanismRatio = sensorToMechanismRatio; + this.offset = offset; + } + + public AbsoluteEncoderConfigBuilder ccwPositive(boolean ccwPositive) { + this.ccwPositive = ccwPositive; + return this; + } + + public AbsoluteEncoderConfigBuilder sensorToMechanismRatio(double sensorToMechanismRatio) { + this.sensorToMechanismRatio = sensorToMechanismRatio; + return this; + } + + public AbsoluteEncoderConfigBuilder offset(Rotation2d offset) { + this.offset = offset; + return this; + } + + public AbsoluteEncoderConfig build() { + return new AbsoluteEncoderConfig(ccwPositive, sensorToMechanismRatio, offset); + } } /** diff --git a/src/main/java/frc/lib/config/FeedbackControllerConfig.java b/src/main/java/frc/lib/config/FeedbackControllerConfig.java index c019de9..936ac83 100644 --- a/src/main/java/frc/lib/config/FeedbackControllerConfig.java +++ b/src/main/java/frc/lib/config/FeedbackControllerConfig.java @@ -1,146 +1,103 @@ package frc.lib.config; import edu.wpi.first.math.controller.PIDController; +import java.util.Objects; /** Feedback controller config. */ -public class FeedbackControllerConfig { - - /** Feedback controller proportional gain. */ - private double kP = 0.0; +public record FeedbackControllerConfig( + double kP, + double kI, + double kD, + boolean continuous, + double positionTolerance, + double velocityTolerance) { + + public FeedbackControllerConfig { + Objects.requireNonNull(kP); + Objects.requireNonNull(kI); + Objects.requireNonNull(kD); + Objects.requireNonNull(continuous); + Objects.requireNonNull(positionTolerance); + Objects.requireNonNull(velocityTolerance); + } - /** Feedback controller integral gain. */ - private double kI = 0.0; + public static final class FeedbackControllerConfigBuilder { + private double kP; - /** Feedback controller derivative gain. */ - private double kD = 0.0; + private double kI; - /** Feedback controller continuous input. */ - private boolean continuousInput = false; + private double kD; - /** Feedback controller position tolerance. */ - private double positionTolerance = 0.0; + private boolean continuous; - /** Feedback controller velocity tolerance. */ - private double velocityTolerance = 0.0; + private double positionTolerance; - /** - * Modifies this controller config's proportional gain. - * - * @param kP the proportional gain. - * @return this controller config. - */ - public FeedbackControllerConfig withProportionalGain(double kP) { - this.kP = kP; - return this; - } + private double velocityTolerance; - /** - * Modifies this controller config's integral gain. - * - * @param kI the integral gain. - * @return this controller config. - */ - public FeedbackControllerConfig withIntegralGain(double kI) { - this.kI = kI; - return this; - } - - /** - * Modifies this controller config's derivative gain. - * - * @param kD the derivative gain. - * @return this controller config. - */ - public FeedbackControllerConfig withDerivativeGain(double kD) { - this.kD = kD; - return this; - } + public static FeedbackControllerConfigBuilder defaults() { + return new FeedbackControllerConfigBuilder(0.0, 0.0, 0.0, false, 0.0, 0.0); + } - /** - * Modifies this controller config's continuous input. - * - * @param continuousInput the continuous input. - * @return this controller config. - */ - public FeedbackControllerConfig withContinuousInput(boolean continuousInput) { - this.continuousInput = continuousInput; - return this; - } + public static FeedbackControllerConfigBuilder from( + FeedbackControllerConfig feedbackControllerConfig) { + return new FeedbackControllerConfigBuilder( + feedbackControllerConfig.kP, + feedbackControllerConfig.kI, + feedbackControllerConfig.kD, + feedbackControllerConfig.continuous, + feedbackControllerConfig.positionTolerance, + feedbackControllerConfig.velocityTolerance); + } - /** - * Modifies this controller config's position tolerance. - * - * @param positionTolerance the position tolerance. - * @return this controller config. - */ - public FeedbackControllerConfig withPositionTolerance(double positionTolerance) { - this.positionTolerance = positionTolerance; - return this; - } + private FeedbackControllerConfigBuilder( + double kP, + double kI, + double kD, + boolean continuous, + double positionTolerance, + double velocityTolerance) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.continuous = continuous; + this.positionTolerance = positionTolerance; + this.velocityTolerance = velocityTolerance; + } - /** - * Modifies this controller config's velocity tolerance. - * - * @param velocityTolerance the velocity tolerance. - * @return this controller config. - */ - public FeedbackControllerConfig withVelocityTolerance(double velocityTolerance) { - this.velocityTolerance = velocityTolerance; - return this; - } + public FeedbackControllerConfigBuilder kP(double kP) { + this.kP = kP; + return this; + } - /** - * Returns the feedback controller proportional gain. - * - * @return the feedback controller proportional gain. - */ - public double kP() { - return kP; - } + public FeedbackControllerConfigBuilder kI(double kI) { + this.kI = kI; + return this; + } - /** - * Returns the feedback controller integral gain. - * - * @return the feedback controller integral gain. - */ - public double kI() { - return kI; - } + public FeedbackControllerConfigBuilder kD(double kD) { + this.kD = kD; + return this; + } - /** - * Returns the feedback controller derivative gain. - * - * @return the feedback controller derivative gain. - */ - public double kD() { - return kD; - } + public FeedbackControllerConfigBuilder continuous(boolean continuous) { + this.continuous = continuous; + return this; + } - /** - * Returns the feedback controller continuous input. - * - * @return the feedback controller continuous input. - */ - public boolean continuousInput() { - return continuousInput; - } + public FeedbackControllerConfigBuilder positionTolerance(double positionTolerance) { + this.positionTolerance = positionTolerance; + return this; + } - /** - * Returns the feedback controller position tolerance. - * - * @return the feedback controller position tolerance. - */ - public double positionTolerance() { - return positionTolerance; - } + public FeedbackControllerConfigBuilder velocityTolerance(double velocityTolerance) { + this.velocityTolerance = velocityTolerance; + return this; + } - /** - * Returns the feedback controller velocity tolerance. - * - * @return the feedback controller velocity tolerance. - */ - public double velocityTolerance() { - return velocityTolerance; + public FeedbackControllerConfig build() { + return new FeedbackControllerConfig( + kP, kI, kD, continuous, positionTolerance, velocityTolerance); + } } /** @@ -153,7 +110,7 @@ public PIDController createPIDController() { pidController.setTolerance(positionTolerance, velocityTolerance); - if (continuousInput) { + if (continuous) { pidController.enableContinuousInput(-0.5, 0.5); } diff --git a/src/main/java/frc/lib/config/FeedforwardControllerConfig.java b/src/main/java/frc/lib/config/FeedforwardControllerConfig.java index 9d9b713..04867fd 100644 --- a/src/main/java/frc/lib/config/FeedforwardControllerConfig.java +++ b/src/main/java/frc/lib/config/FeedforwardControllerConfig.java @@ -2,63 +2,70 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import java.util.Objects; /** Feedforward controller config. */ -public class FeedforwardControllerConfig { - /** Feedforward controller static gain. */ - private double kS = 0.0; +public record FeedforwardControllerConfig(double kS, double kG, double kV, double kA) { - /** Feedforward controller gravity gain. */ - private double kG = 0.0; + public FeedforwardControllerConfig { + Objects.requireNonNull(kS); + Objects.requireNonNull(kG); + Objects.requireNonNull(kV); + Objects.requireNonNull(kA); + } - /** Feedforward controller velocity gain. */ - private double kV = 0.0; + public static final class FeedforwardControllerConfigBuilder { + private double kS; - /** Feedforward controller acceleration gain. */ - private double kA = 0.0; + private double kG; - /** - * Modifies this controller config's static feedforward. - * - * @param kS the static feedforward. - * @return this controller config. - */ - public FeedforwardControllerConfig withStaticFeedforward(double kS) { - this.kS = kS; - return this; - } + private double kV; - /** - * Modifies this controller config's gravity feedforward. - * - * @param kG the gravity feedforward. - * @return this controller config. - */ - public FeedforwardControllerConfig withGravityFeedforward(double kG) { - this.kG = kG; - return this; - } + private double kA; - /** - * Modifies this controller config's velocity feedforward. - * - * @param kV the velocity feedforward. - * @return this controller config. - */ - public FeedforwardControllerConfig withVelocityFeedforward(double kV) { - this.kV = kV; - return this; - } + public static FeedforwardControllerConfigBuilder defaults() { + return new FeedforwardControllerConfigBuilder(0.0, 0.0, 0.0, 0.0); + } - /** - * Modifies this controller config's acceleration feedforward. - * - * @param kA the acceleration feedforward. - * @return this controller config. - */ - public FeedforwardControllerConfig withAccelerationFeedfoward(double kA) { - this.kA = kA; - return this; + public static FeedforwardControllerConfigBuilder from( + FeedforwardControllerConfig feedforwardControllerConfig) { + return new FeedforwardControllerConfigBuilder( + feedforwardControllerConfig.kS, + feedforwardControllerConfig.kG, + feedforwardControllerConfig.kV, + feedforwardControllerConfig.kA); + } + + private FeedforwardControllerConfigBuilder(double kS, double kG, double kV, double kA) { + this.kS = kS; + this.kG = kG; + this.kV = kV; + this.kA = kA; + } + + public FeedforwardControllerConfigBuilder kS(double kS) { + this.kS = kS; + return this; + } + + public FeedforwardControllerConfigBuilder kG(double kG) { + this.kG = kG; + return this; + } + + public FeedforwardControllerConfigBuilder kV(double kV) { + this.kV = kV; + return this; + } + + public FeedforwardControllerConfigBuilder kA(double kA) { + this.kA = kA; + return this; + } + + public FeedforwardControllerConfig build() { + return new FeedforwardControllerConfig(kS, kG, kV, kA); + } } /** @@ -82,40 +89,4 @@ public SimpleMotorFeedforward createSimpleMotorFeedforward() { public ArmFeedforward createArmFeedforward() { return new ArmFeedforward(kS, kG, kV, kA); } - - /** - * Returns the feedforward controller static gain. - * - * @return the feedforward controller static gain. - */ - public double kS() { - return kS; - } - - /** - * Returns the feedforward controller gravity gain. - * - * @return the feedforward controller gravity gain. - */ - public double kG() { - return kG; - } - - /** - * Returns the feedforward controller velocity gain. - * - * @return the feedforward controller velocity gain. - */ - public double kV() { - return kV; - } - - /** - * Returns the feedforward controller acceleration gain. - * - * @return the feedforward controller acceleration gain. - */ - public double kA() { - return kA; - } } diff --git a/src/main/java/frc/lib/config/MechanismConfig.java b/src/main/java/frc/lib/config/MechanismConfig.java index 71fabc8..e3c1086 100644 --- a/src/main/java/frc/lib/config/MechanismConfig.java +++ b/src/main/java/frc/lib/config/MechanismConfig.java @@ -1,122 +1,113 @@ package frc.lib.config; -/** Mechanism config. */ -public class MechanismConfig { - - /** Absolute encoder config. */ - private AbsoluteEncoderConfig absoluteEncoderConfig = new AbsoluteEncoderConfig(); - - /** Feedback controller config. */ - private FeedbackControllerConfig feedbackControllerConfig = new FeedbackControllerConfig(); - - /** Feedforward controller config. */ - private FeedforwardControllerConfig feedforwardControllerConfig = - new FeedforwardControllerConfig(); - - /** Motion profile config. */ - private MotionProfileConfig motionProfileConfig = new MotionProfileConfig(); - - /** Motor config. */ - private MotorConfig motorConfig = new MotorConfig(); - - /** - * Modifies this mechanism config to use the absolute encoder config. - * - * @param absoluteEncoderConfig the absolute encoder config. - * @return this mechanism config. - */ - public MechanismConfig withAbsoluteEncoderConfig(AbsoluteEncoderConfig absoluteEncoderConfig) { - this.absoluteEncoderConfig = absoluteEncoderConfig; - return this; - } - - /** - * Modifies this mechanism config to use the feedback controller config. - * - * @param feedbackControllerConfig the feedback controller config. - * @return this mechanism config. - */ - public MechanismConfig withFeedbackConfig(FeedbackControllerConfig feedbackControllerConfig) { - this.feedbackControllerConfig = feedbackControllerConfig; - return this; - } - - /** - * Modifies this mechanism config to use the feedforward controller config. - * - * @param feedforwardControllerConfig the feedforward controller config. - * @return this mechanism config. - */ - public MechanismConfig withFeedforwardConfig( - FeedforwardControllerConfig feedforwardControllerConfig) { - this.feedforwardControllerConfig = feedforwardControllerConfig; - return this; - } +import frc.lib.config.AbsoluteEncoderConfig.AbsoluteEncoderConfigBuilder; +import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; +import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; +import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; +import frc.lib.config.MotorConfig.MotorConfigBuilder; +import java.util.Objects; - /** - * Modifies this mechanism config to use the motion profile config. - * - * @param motionProfileConfig the motion profile config. - * @return this mechanism config. - */ - public MechanismConfig withMotionProfileConfig(MotionProfileConfig motionProfileConfig) { - this.motionProfileConfig = motionProfileConfig; - return this; - } - - /** - * Modifies this mechanism config to use the motor config. - * - * @param motorConfig the motor config. - * @return this mechanism config. - */ - public MechanismConfig withMotorConfig(MotorConfig motorConfig) { - this.motorConfig = motorConfig; - return this; - } - - /** - * Returns the absolute encoder config. - * - * @return the absolute encoder config. - */ - public AbsoluteEncoderConfig absoluteEncoderConfig() { - return absoluteEncoderConfig; - } - - /** - * Returns the feedback controller config. - * - * @return the feedback controller config. - */ - public FeedbackControllerConfig feedbackControllerConfig() { - return feedbackControllerConfig; - } - - /** - * Returns the feedforward controller config. - * - * @return the feedforward controller config. - */ - public FeedforwardControllerConfig feedforwardControllerConfig() { - return feedforwardControllerConfig; - } - - /** - * Returns the motion profile config. - * - * @return the motion profile config. - */ - public MotionProfileConfig motionProfileConfig() { - return motionProfileConfig; +/** Mechanism config. */ +public record MechanismConfig( + AbsoluteEncoderConfig absoluteEncoderConfig, + FeedbackControllerConfig feedbackControllerConfig, + FeedforwardControllerConfig feedforwardControllerConfig, + MotionProfileConfig motionProfileConfig, + MotorConfig motorConfig) { + + public MechanismConfig { + Objects.requireNonNull(absoluteEncoderConfig); + Objects.requireNonNull(feedbackControllerConfig); + Objects.requireNonNull(feedforwardControllerConfig); + Objects.requireNonNull(motionProfileConfig); + Objects.requireNonNull(motorConfig); } - /** - * Returns the motor config. - * - * @return the motor config. - */ - public MotorConfig motorConfig() { - return motorConfig; + /** Mechanism config builder. */ + public static final class MechanismConfigBuilder { + + private AbsoluteEncoderConfigBuilder absoluteEncoderConfigBuilder; + + private FeedbackControllerConfigBuilder feedbackControllerConfigBuilder; + + private FeedforwardControllerConfigBuilder feedforwardControllerConfigBuilder; + + private MotionProfileConfigBuilder motionProfileConfigBuilder; + + private MotorConfigBuilder motorConfigBuilder; + + public static MechanismConfigBuilder defaults() { + return new MechanismConfigBuilder( + AbsoluteEncoderConfigBuilder.defaults(), + FeedbackControllerConfigBuilder.defaults(), + FeedforwardControllerConfigBuilder.defaults(), + MotionProfileConfigBuilder.defaults(), + MotorConfigBuilder.defaults()); + } + + public static MechanismConfigBuilder from(MechanismConfig mechanismConfig) { + return new MechanismConfigBuilder( + AbsoluteEncoderConfigBuilder.from(mechanismConfig.absoluteEncoderConfig), + FeedbackControllerConfigBuilder.from(mechanismConfig.feedbackControllerConfig), + FeedforwardControllerConfigBuilder.from(mechanismConfig.feedforwardControllerConfig), + MotionProfileConfigBuilder.from(mechanismConfig.motionProfileConfig), + MotorConfigBuilder.from(mechanismConfig.motorConfig)); + } + + private MechanismConfigBuilder( + AbsoluteEncoderConfigBuilder absoluteEncoderConfigBuilder, + FeedbackControllerConfigBuilder feedbackControllerConfigBuilder, + FeedforwardControllerConfigBuilder feedforwardControllerConfigBuilder, + MotionProfileConfigBuilder motionProfileConfigBuilder, + MotorConfigBuilder motorConfigBuilder) { + this.absoluteEncoderConfigBuilder = absoluteEncoderConfigBuilder; + this.feedbackControllerConfigBuilder = feedbackControllerConfigBuilder; + this.feedforwardControllerConfigBuilder = feedforwardControllerConfigBuilder; + this.motionProfileConfigBuilder = motionProfileConfigBuilder; + this.motorConfigBuilder = motorConfigBuilder; + } + + public MechanismConfigBuilder absoluteEncoderConfig( + AbsoluteEncoderConfigBuilder absoluteEncoderConfigBuilder) { + this.absoluteEncoderConfigBuilder = absoluteEncoderConfigBuilder; + return this; + } + + public MechanismConfigBuilder feedbackControllerConfig( + FeedbackControllerConfigBuilder feedbackControllerConfigBuilder) { + this.feedbackControllerConfigBuilder = feedbackControllerConfigBuilder; + return this; + } + + public MechanismConfigBuilder feedforwardControllerConfig( + FeedforwardControllerConfigBuilder feedforwardControllerConfigBuilder) { + this.feedforwardControllerConfigBuilder = feedforwardControllerConfigBuilder; + return this; + } + + public MechanismConfigBuilder motionProfileConfig( + MotionProfileConfigBuilder motionProfileConfigBuilder) { + this.motionProfileConfigBuilder = motionProfileConfigBuilder; + return this; + } + + public MechanismConfigBuilder motorConfig(MotorConfigBuilder motorConfigBuilder) { + this.motorConfigBuilder = motorConfigBuilder; + return this; + } + + /** + * Returns the built mechanism config. + * + * @return the built mechanism config. + */ + public MechanismConfig build() { + return new MechanismConfig( + absoluteEncoderConfigBuilder.build(), + feedbackControllerConfigBuilder.build(), + feedforwardControllerConfigBuilder.build(), + motionProfileConfigBuilder.build(), + motorConfigBuilder.build()); + } } } diff --git a/src/main/java/frc/lib/config/MotionProfileConfig.java b/src/main/java/frc/lib/config/MotionProfileConfig.java index ffdf644..2f65804 100644 --- a/src/main/java/frc/lib/config/MotionProfileConfig.java +++ b/src/main/java/frc/lib/config/MotionProfileConfig.java @@ -4,16 +4,51 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import java.util.Objects; import java.util.function.Function; /** Motion profile config. */ -public class MotionProfileConfig { +public record MotionProfileConfig(double maximumVelocity, double maximumAcceleration) { - /** Maximum velocity. */ - private double maximumVelocity = 0.0; + public MotionProfileConfig { + Objects.requireNonNull(maximumVelocity); + Objects.requireNonNull(maximumAcceleration); + } + + public static final class MotionProfileConfigBuilder { + + private double maximumVelocity; + + private double maximumAcceleration; + + public static MotionProfileConfigBuilder defaults() { + return new MotionProfileConfigBuilder(0.0, 0.0); + } + + public static MotionProfileConfigBuilder from(MotionProfileConfig motionProfileConfig) { + return new MotionProfileConfigBuilder( + motionProfileConfig.maximumVelocity, motionProfileConfig.maximumAcceleration); + } + + private MotionProfileConfigBuilder(double maximumVelocity, double maximumAcceleration) { + this.maximumVelocity = maximumVelocity; + this.maximumAcceleration = maximumAcceleration; + } + + public MotionProfileConfigBuilder maximumVelocity(double maximumVelocity) { + this.maximumVelocity = maximumVelocity; + return this; + } - /** Maximum acceleration. */ - private double maximumAcceleration = 0.0; + public MotionProfileConfigBuilder maximumAcceleration(double maximumAcceleration) { + this.maximumAcceleration = maximumAcceleration; + return this; + } + + public MotionProfileConfig build() { + return new MotionProfileConfig(maximumVelocity, maximumAcceleration); + } + } /** * Calculates an acceleration using a ramp duration. @@ -27,46 +62,6 @@ public static double calculateAcceleration( return maximumSpeed / desiredRampDurationSeconds; } - /** - * Modifies this motion profile config's maximum velocity. - * - * @param maximumVelocity the maximum velocity. - * @return this motion profile config. - */ - public MotionProfileConfig withMaximumVelocity(double maximumVelocity) { - this.maximumVelocity = maximumVelocity; - return this; - } - - /** - * Modifies this motion profile config's maximum acceleration. - * - * @param maximumAcceleration the maximum acceleration. - * @return this motion profile config. - */ - public MotionProfileConfig withMaximumAcceleration(double maximumAcceleration) { - this.maximumAcceleration = maximumAcceleration; - return this; - } - - /** - * Returns the motion profile maximum velocity. - * - * @return the motion profile maximum velocity. - */ - public double maximumVelocity() { - return maximumVelocity; - } - - /** - * Returns the motion profile maximum acceleration. - * - * @return the motion profile maximum acceleration. - */ - public double maximumAcceleration() { - return maximumAcceleration; - } - /** * Creates a new velocity clamper using this motion profile config. * diff --git a/src/main/java/frc/lib/config/MotorConfig.java b/src/main/java/frc/lib/config/MotorConfig.java index 0b82a03..cac5b30 100644 --- a/src/main/java/frc/lib/config/MotorConfig.java +++ b/src/main/java/frc/lib/config/MotorConfig.java @@ -5,100 +5,77 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import java.util.Objects; /** Motor config. */ -public class MotorConfig { - - /** Use the motor to brake the controller mechanism on neutral output. */ - private boolean neutralBrake = false; - - /** Interpret counterclockwise rotation on the motor face as having positive velocity. */ - private boolean ccwPositive = true; - - /** Ratio between the motor and the mechanism. */ - private double motorToMechanismRatio = 1.0; - - /** Maximum amount of current, in amps, to provide to the motor. */ - private double currentLimitAmps = 40.0; - - /** - * Modifies this motor config's neutral brake. - * - * @param neutralBrake the neutral brake. - * @return this motor config. - */ - public MotorConfig withNeutralBrake(boolean neutralBrake) { - this.neutralBrake = neutralBrake; - return this; +public record MotorConfig( + boolean neutralBrake, + boolean ccwPositive, + double motorToMechanismRatio, + double currentLimitAmps) { + + public MotorConfig { + Objects.requireNonNull(neutralBrake); + Objects.requireNonNull(ccwPositive); + Objects.requireNonNull(motorToMechanismRatio); + Objects.requireNonNull(currentLimitAmps); } - /** - * Modifies this motor config's counterclockwise positive. - * - * @param ccwPositive the counterclockwise positive. - * @return this motor config. - */ - public MotorConfig withCCWPositive(boolean ccwPositive) { - this.ccwPositive = ccwPositive; - return this; - } - - /** - * Modifies this motor config's motor to mechanism ratio. - * - * @param motorToMechanismRatio the motor to mechanism ratio. - * @return this motor config. - */ - public MotorConfig withMotorToMechanismRatio(double motorToMechanismRatio) { - this.motorToMechanismRatio = motorToMechanismRatio; - return this; - } - - /** - * Modifies this motor config's current limit. - * - * @param currentLimitAmps the current limit. - * @return this motor config. - */ - public MotorConfig withCurrentLimit(double currentLimitAmps) { - this.currentLimitAmps = currentLimitAmps; - return this; - } - - /** - * Returns true if motor should neutral brake. - * - * @return true if motor should neutral brake. - */ - public boolean neutralBrake() { - return neutralBrake; - } - - /** - * Returns true if the motor is counterclockwise positive. - * - * @return true if the motor is counterclockwise positive. - */ - public boolean ccwPositive() { - return ccwPositive; - } - - /** - * Returns the motor to mechanism ratio. - * - * @return the motor to mechansim ratio. - */ - public double motorToMechanismRatio() { - return motorToMechanismRatio; - } - - /** - * Returns the motor current limit, in amps. - * - * @return the motor current limit, in amps. - */ - public double currentLimitAmps() { - return currentLimitAmps; + public static final class MotorConfigBuilder { + private boolean neutralBrake; + + private boolean ccwPositive; + + private double motorToMechanismRatio; + + private double currentLimitAmps; + + public static MotorConfigBuilder defaults() { + return new MotorConfigBuilder(false, true, 1.0, 40.0); + } + + public static MotorConfigBuilder from(MotorConfig motorConfig) { + return new MotorConfigBuilder( + motorConfig.neutralBrake, + motorConfig.ccwPositive, + motorConfig.motorToMechanismRatio, + motorConfig.currentLimitAmps); + } + + private MotorConfigBuilder( + boolean neutralBrake, + boolean ccwPositive, + double motorToMechanismRatio, + double currentLimitAmps) { + this.neutralBrake = neutralBrake; + this.ccwPositive = ccwPositive; + this.motorToMechanismRatio = motorToMechanismRatio; + this.currentLimitAmps = currentLimitAmps; + } + + public MotorConfigBuilder neutralBrake(boolean neutralBrake) { + this.neutralBrake = neutralBrake; + return this; + } + + public MotorConfigBuilder ccwPositive(boolean ccwPositive) { + this.ccwPositive = ccwPositive; + return this; + } + + public MotorConfigBuilder motorToMechanismRatio(double motorToMechanismRatio) { + this.motorToMechanismRatio = motorToMechanismRatio; + return this; + } + + public MotorConfigBuilder currentLimitAmps(double currentLimitAmps) { + this.currentLimitAmps = currentLimitAmps; + return this; + } + + public MotorConfig build() { + return new MotorConfig(neutralBrake, ccwPositive, motorToMechanismRatio, currentLimitAmps); + } } /** diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 535b05f..cbc1a3c 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -7,12 +7,13 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.lib.config.AbsoluteEncoderConfig; -import frc.lib.config.FeedbackControllerConfig; -import frc.lib.config.FeedforwardControllerConfig; +import frc.lib.config.AbsoluteEncoderConfig.AbsoluteEncoderConfigBuilder; +import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; +import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; import frc.lib.config.MechanismConfig; -import frc.lib.config.MotionProfileConfig; -import frc.lib.config.MotorConfig; +import frc.lib.config.MechanismConfig.MechanismConfigBuilder; +import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; +import frc.lib.config.MotorConfig.MotorConfigBuilder; import frc.lib.controller.PositionControllerIO; import frc.lib.controller.PositionControllerIO.PositionControllerIOValues; @@ -24,31 +25,24 @@ public class Arm extends Subsystem { /** Shoulder controller config. */ private final MechanismConfig shoulderConfig = - new MechanismConfig() - .withAbsoluteEncoderConfig( - new AbsoluteEncoderConfig() - .withCCWPositive(false) - .withOffset(Rotation2d.fromDegrees(-173.135))) - .withMotorConfig( - new MotorConfig() - .withCCWPositive(true) - .withNeutralBrake(true) - .withMotorToMechanismRatio(39.771428571)) - .withMotionProfileConfig( - new MotionProfileConfig() - .withMaximumVelocity(Units.degreesToRotations(240.0)) // rotations per second - .withMaximumAcceleration( - Units.degreesToRadians(240.0)) // rotations per second per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.14) // volts - .withGravityFeedforward(0.5125) // volts - .withVelocityFeedforward(4.0) // volts per rotation per second - ) - .withFeedbackConfig( - new FeedbackControllerConfig().withProportionalGain(4.0) // volts per rotation - ); + MechanismConfigBuilder.defaults() + .absoluteEncoderConfig( + AbsoluteEncoderConfigBuilder.defaults() + .ccwPositive(false) + .offset(Rotation2d.fromDegrees(-173.135))) + .motorConfig( + MotorConfigBuilder.defaults() + .ccwPositive(true) + .neutralBrake(true) + .motorToMechanismRatio(39.771428571)) + .motionProfileConfig( + MotionProfileConfigBuilder.defaults() + .maximumVelocity(Units.degreesToRotations(240.0)) + .maximumAcceleration(Units.degreesToRotations(240.0))) + .feedforwardControllerConfig( + FeedforwardControllerConfigBuilder.defaults().kS(0.14).kG(0.5125).kV(4.0)) + .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(4.0)) + .build(); /** Shoulder controller. */ private final PositionControllerIO shoulder; diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 3f1b608..cd365e5 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -3,11 +3,12 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.Subsystem; -import frc.lib.config.FeedbackControllerConfig; -import frc.lib.config.FeedforwardControllerConfig; +import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; +import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; import frc.lib.config.MechanismConfig; -import frc.lib.config.MotionProfileConfig; -import frc.lib.config.MotorConfig; +import frc.lib.config.MechanismConfig.MechanismConfigBuilder; +import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; +import frc.lib.config.MotorConfig.MotorConfigBuilder; import frc.lib.controller.VelocityControllerIO; import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; @@ -19,45 +20,31 @@ public class Intake extends Subsystem { /** Front roller controller config. */ private final MechanismConfig frontRollerConfig = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(false) - .withNeutralBrake(false) - .withMotorToMechanismRatio(24.0 / 16.0)) - .withMotionProfileConfig( - new MotionProfileConfig().withMaximumVelocity(66) // rotations per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.13) // volts - .withVelocityFeedforward(0.1683) // volts per rotation per second - ) - .withFeedbackConfig( - new FeedbackControllerConfig() - .withProportionalGain(0.1) // volts per rotation per second - ); + MechanismConfigBuilder.defaults() + .motorConfig( + MotorConfigBuilder.defaults() + .ccwPositive(false) + .neutralBrake(false) + .motorToMechanismRatio(24.0 / 16.0)) + .motionProfileConfig(MotionProfileConfigBuilder.defaults().maximumVelocity(66)) + .feedforwardControllerConfig( + FeedforwardControllerConfigBuilder.defaults().kS(0.13).kV(0.1683)) + .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(0.1)) + .build(); /** Back roller controller config. */ private final MechanismConfig backRollerConfig = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(false) - .withNeutralBrake(false) - .withMotorToMechanismRatio(24.0 / 16.0)) - .withMotionProfileConfig( - new MotionProfileConfig().withMaximumVelocity(66) // rotations per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.13) // volts - .withVelocityFeedforward(0.1759) // volts per rotation per second - ) - .withFeedbackConfig( - new FeedbackControllerConfig() - .withProportionalGain(0.1) // volts per rotation per second - ); + MechanismConfigBuilder.defaults() + .motorConfig( + MotorConfigBuilder.defaults() + .ccwPositive(false) + .neutralBrake(false) + .motorToMechanismRatio(24.0 / 16.0)) + .motionProfileConfig(MotionProfileConfigBuilder.defaults().maximumVelocity(66)) + .feedforwardControllerConfig( + FeedforwardControllerConfigBuilder.defaults().kS(0.13).kV(0.1759)) + .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(0.1)) + .build(); /** Rollers. */ private final VelocityControllerIO frontRoller, backRoller; diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index f95eb91..b9c626a 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -4,11 +4,12 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.Subsystem; -import frc.lib.config.FeedbackControllerConfig; -import frc.lib.config.FeedforwardControllerConfig; +import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; +import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; import frc.lib.config.MechanismConfig; -import frc.lib.config.MotionProfileConfig; -import frc.lib.config.MotorConfig; +import frc.lib.config.MechanismConfig.MechanismConfigBuilder; +import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; +import frc.lib.config.MotorConfig.MotorConfigBuilder; import frc.lib.controller.VelocityControllerIO; import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; @@ -20,26 +21,18 @@ public class Shooter extends Subsystem { /** Flywheel controller config. */ private final MechanismConfig flywheelConfig = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(false) - .withNeutralBrake(true) - .withMotorToMechanismRatio(28.0 / 16.0)) - .withMotionProfileConfig( - new MotionProfileConfig() - .withMaximumVelocity(60) // rotations per second - .withMaximumAcceleration(200) // rotations per second per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.14) // volts - .withVelocityFeedforward(0.2) // volts per rotation per second - ) - .withFeedbackConfig( - new FeedbackControllerConfig() - .withProportionalGain(0.14) // volts per rotation per second - ); + MechanismConfigBuilder.defaults() + .motorConfig( + MotorConfigBuilder.defaults() + .ccwPositive(false) + .neutralBrake(true) + .motorToMechanismRatio(28.0 / 16.0)) + .motionProfileConfig( + MotionProfileConfigBuilder.defaults().maximumVelocity(60).maximumAcceleration(200)) + .feedforwardControllerConfig( + FeedforwardControllerConfigBuilder.defaults().kS(0.14).kV(0.2)) + .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(0.14)) + .build(); /** Flywheel controller. */ private final VelocityControllerIO flywheel; @@ -52,22 +45,17 @@ public class Shooter extends Subsystem { /** Serializer controller config. */ private final MechanismConfig serializerConfig = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(true) - .withNeutralBrake(false) - .withMotorToMechanismRatio(36.0 / 16.0)) - .withMotionProfileConfig( - new MotionProfileConfig() - .withMaximumVelocity(45) // rotations per second - .withMaximumAcceleration(450) // rotations per second per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.14) // volts - .withVelocityFeedforward(0.2617) // volts per rotation per second - ); + MechanismConfigBuilder.defaults() + .motorConfig( + MotorConfigBuilder.defaults() + .ccwPositive(true) + .neutralBrake(false) + .motorToMechanismRatio(36.0 / 16.0)) + .motionProfileConfig( + MotionProfileConfigBuilder.defaults().maximumVelocity(45).maximumAcceleration(450)) + .feedforwardControllerConfig( + FeedforwardControllerConfigBuilder.defaults().kS(0.14).kV(0.2617)) + .build(); /** Serializer controller. */ private final VelocityControllerIO serializer; diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 6c0fafe..af4ae55 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -14,11 +14,13 @@ import frc.lib.DriveRequest; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.lib.config.FeedbackControllerConfig; -import frc.lib.config.FeedforwardControllerConfig; +import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; +import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; import frc.lib.config.MechanismConfig; +import frc.lib.config.MechanismConfig.MechanismConfigBuilder; import frc.lib.config.MotionProfileConfig; -import frc.lib.config.MotorConfig; +import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; +import frc.lib.config.MotorConfig.MotorConfigBuilder; import frc.lib.controller.SwerveModuleIO; import frc.robot.RobotConstants; import frc.robot.odometry.Odometry; @@ -38,53 +40,44 @@ public class Swerve extends Subsystem { /** Steer motor config. */ private final MechanismConfig steerConfig = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(false) - .withCurrentLimit(20) - .withMotorToMechanismRatio(150.0 / 7.0)) - .withFeedforwardConfig( - new FeedforwardControllerConfig().withStaticFeedforward(0.205) // volts - ) - .withFeedbackConfig( - new FeedbackControllerConfig() - .withContinuousInput(true) - .withProportionalGain(54.0) // volts per rotation - .withDerivativeGain(0.16) // volts per rotation per second - .withPositionTolerance(Units.degreesToRotations(1)) // rotations - ); + MechanismConfigBuilder.defaults() + .motorConfig( + MotorConfigBuilder.defaults() + .ccwPositive(false) + .currentLimitAmps(20) + .motorToMechanismRatio(150.0 / 7.0)) + .feedforwardControllerConfig(FeedforwardControllerConfigBuilder.defaults().kS(0.205)) + .feedbackControllerConfig( + FeedbackControllerConfigBuilder.defaults() + .continuous(true) + .kP(54.0) + .kD(0.16) + .positionTolerance(Units.degreesToRotations(1.0))) + .build(); /** Drive motor config. */ private final MechanismConfig driveConfig = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(false) - .withCurrentLimit(40) - .withMotorToMechanismRatio(6.75)) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.14) // volts - .withVelocityFeedforward(0.725) // volts per rotation per second - ) - .withFeedbackConfig( - new FeedbackControllerConfig() - .withProportionalGain(0.75) // volts per rotation per second - ); + MechanismConfigBuilder.defaults() + .motorConfig( + MotorConfigBuilder.defaults() + .ccwPositive(false) + .currentLimitAmps(40.0) + .motorToMechanismRatio(6.75)) + .feedforwardControllerConfig( + FeedforwardControllerConfigBuilder.defaults().kS(0.14).kV(0.725)) + .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(0.75)) + .build(); /** Wheel circumference. */ private final double wheelCircumference = Units.inchesToMeters(4.0) * Math.PI; /** Translation motion profile config. */ private final MotionProfileConfig translationMotionProfileConfig = - new MotionProfileConfig() - .withMaximumVelocity(4.5) // meters per second - .withMaximumAcceleration(18); // meters per second per second + MotionProfileConfigBuilder.defaults().maximumVelocity(4.5).maximumAcceleration(18).build(); /** Rotation motion profile config. */ private final MotionProfileConfig rotationMotionProfileConfig = - new MotionProfileConfig().withMaximumVelocity(1); // rotations per second + MotionProfileConfigBuilder.defaults().maximumVelocity(1.0).build(); /** Initializes the swerve subsystem and configures swerve hardware. */ private Swerve() { diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java index e532e01..a3d72e1 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -4,8 +4,9 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import frc.lib.CAN; -import frc.lib.config.AbsoluteEncoderConfig; +import frc.lib.config.AbsoluteEncoderConfig.AbsoluteEncoderConfigBuilder; import frc.lib.config.MechanismConfig; +import frc.lib.config.MechanismConfig.MechanismConfigBuilder; import frc.lib.controller.PositionControllerIO; import frc.lib.controller.PositionControllerIOSim; import frc.lib.controller.PositionControllerIOTalonFXSteer; @@ -50,9 +51,11 @@ public static SwerveModuleIO createNorthWestModule( new CAN(8, "swerve"), new CAN(16, "swerve"), new CAN(24, "swerve"), - steerConfig.withAbsoluteEncoderConfig( - new AbsoluteEncoderConfig() - .withOffset(Rotation2d.fromRotations(-0.084717).unaryMinus())), + MechanismConfigBuilder.from(steerConfig) + .absoluteEncoderConfig( + AbsoluteEncoderConfigBuilder.defaults() + .offset(Rotation2d.fromRotations(-0.084716).unaryMinus())) + .build(), driveConfig, wheelCircumference); } @@ -77,9 +80,11 @@ public static SwerveModuleIO createNorthEastModule( new CAN(16, "swerve"), new CAN(18, "swerve"), new CAN(30, "swerve"), - steerConfig.withAbsoluteEncoderConfig( - new AbsoluteEncoderConfig() - .withOffset(Rotation2d.fromRotations(0.196777).unaryMinus())), + MechanismConfigBuilder.from(steerConfig) + .absoluteEncoderConfig( + AbsoluteEncoderConfigBuilder.defaults() + .offset(Rotation2d.fromRotations(0.196777).unaryMinus())) + .build(), driveConfig, wheelCircumference); } @@ -104,9 +109,11 @@ public static SwerveModuleIO createSouthEastModule( new CAN(12, "swerve"), new CAN(22, "swerve"), new CAN(26, "swerve"), - steerConfig.withAbsoluteEncoderConfig( - new AbsoluteEncoderConfig() - .withOffset(Rotation2d.fromRotations(0.276611).unaryMinus())), + MechanismConfigBuilder.from(steerConfig) + .absoluteEncoderConfig( + AbsoluteEncoderConfigBuilder.defaults() + .offset(Rotation2d.fromRotations(0.276611).unaryMinus())) + .build(), driveConfig, wheelCircumference); } @@ -131,9 +138,11 @@ public static SwerveModuleIO createSouthWestModule( new CAN(10, "swerve"), new CAN(20, "swerve"), new CAN(28, "swerve"), - steerConfig.withAbsoluteEncoderConfig( - new AbsoluteEncoderConfig() - .withOffset(Rotation2d.fromRotations(0.223145).unaryMinus())), + MechanismConfigBuilder.from(steerConfig) + .absoluteEncoderConfig( + AbsoluteEncoderConfigBuilder.defaults() + .offset(Rotation2d.fromRotations(0.223145).unaryMinus())) + .build(), driveConfig, wheelCircumference); }