Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
refactor: Rewrite configs using the builder pattern.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed May 4, 2024
1 parent 645f7b0 commit d722003
Show file tree
Hide file tree
Showing 11 changed files with 533 additions and 684 deletions.
119 changes: 53 additions & 66 deletions src/main/java/frc/lib/config/AbsoluteEncoderConfig.java
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

/**
Expand Down
203 changes: 80 additions & 123 deletions src/main/java/frc/lib/config/FeedbackControllerConfig.java
Original file line number Diff line number Diff line change
@@ -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);
}
}

/**
Expand All @@ -153,7 +110,7 @@ public PIDController createPIDController() {

pidController.setTolerance(positionTolerance, velocityTolerance);

if (continuousInput) {
if (continuous) {
pidController.enableContinuousInput(-0.5, 0.5);
}

Expand Down
Loading

0 comments on commit d722003

Please sign in to comment.