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

Commit

Permalink
refactor(config): Use method calls for values.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 25, 2024
1 parent ae6219e commit 4a21d0c
Show file tree
Hide file tree
Showing 11 changed files with 103 additions and 59 deletions.
62 changes: 50 additions & 12 deletions src/main/java/frc/lib/config/MechanismConfig.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,25 +4,26 @@
public class MechanismConfig {

/** Absolute encoder config. */
public AbsoluteEncoderConfig absoluteEncoder = new AbsoluteEncoderConfig();
private AbsoluteEncoderConfig absoluteEncoderConfig = new AbsoluteEncoderConfig();

/** Feedback controller config. */
public FeedbackControllerConfig feedback = new FeedbackControllerConfig();
private FeedbackControllerConfig feedbackControllerConfig = new FeedbackControllerConfig();

/** Feedforward controller config. */
public FeedforwardControllerConfig feedforward = new FeedforwardControllerConfig();
private FeedforwardControllerConfig feedforwardControllerConfig =
new FeedforwardControllerConfig();

/** Motor config. */
public MotorConfig motor = new MotorConfig();
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 withAbsoluteEncoder(AbsoluteEncoderConfig absoluteEncoderConfig) {
this.absoluteEncoder = absoluteEncoderConfig;
public MechanismConfig withAbsoluteEncoderConfig(AbsoluteEncoderConfig absoluteEncoderConfig) {
this.absoluteEncoderConfig = absoluteEncoderConfig;
return this;
}

Expand All @@ -32,8 +33,8 @@ public MechanismConfig withAbsoluteEncoder(AbsoluteEncoderConfig absoluteEncoder
* @param feedbackControllerConfig the feedback controller config.
* @return this mechanism config.
*/
public MechanismConfig withFeedback(FeedbackControllerConfig feedbackControllerConfig) {
this.feedback = feedbackControllerConfig;
public MechanismConfig withFeedbackConfig(FeedbackControllerConfig feedbackControllerConfig) {
this.feedbackControllerConfig = feedbackControllerConfig;
return this;
}

Expand All @@ -43,8 +44,9 @@ public MechanismConfig withFeedback(FeedbackControllerConfig feedbackControllerC
* @param feedforwardControllerConfig the feedforward controller config.
* @return this mechanism config.
*/
public MechanismConfig withFeedforward(FeedforwardControllerConfig feedforwardControllerConfig) {
this.feedforward = feedforwardControllerConfig;
public MechanismConfig withFeedforwardConfig(
FeedforwardControllerConfig feedforwardControllerConfig) {
this.feedforwardControllerConfig = feedforwardControllerConfig;
return this;
}

Expand All @@ -54,8 +56,44 @@ public MechanismConfig withFeedforward(FeedforwardControllerConfig feedforwardCo
* @param motorConfig the motor config.
* @return this mechanism config.
*/
public MechanismConfig withMotor(MotorConfig motorConfig) {
this.motor = motorConfig;
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 motor config.
*
* @return the motor config.
*/
public MotorConfig motorConfig() {
return motorConfig;
}
}
22 changes: 12 additions & 10 deletions src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java
Original file line number Diff line number Diff line change
Expand Up @@ -69,9 +69,9 @@ public PositionControllerIOTalonFX2(
volts = leaderMotor.getMotorVoltage();
amps = leaderMotor.getStatorCurrent();

feedforward = config.feedforward.createArmFeedforward();
feedforward = config.feedforwardControllerConfig().createArmFeedforward();

feedback = config.feedback.createPIDController();
feedback = config.feedbackControllerConfig().createPIDController();

followerMotor.setControl(new Follower(leaderMotor.getDeviceID(), invertFollower));

Expand All @@ -87,34 +87,36 @@ public void configure() {
TalonFXConfiguration motorConfig = new TalonFXConfiguration();

motorConfig.MotorOutput.Inverted =
config.motor.ccwPositive()
config.motorConfig().ccwPositive()
? InvertedValue.CounterClockwise_Positive
: InvertedValue.Clockwise_Positive;
motorConfig.MotorOutput.NeutralMode =
config.motor.neutralBrake() ? NeutralModeValue.Brake : NeutralModeValue.Coast;
config.motorConfig().neutralBrake() ? NeutralModeValue.Brake : NeutralModeValue.Coast;

// Stator current is a measure of the current inside of the motor and is typically higher than
// supply (breaker) current
motorConfig.CurrentLimits.StatorCurrentLimit = config.motor.currentLimitAmps() * 2.0;
motorConfig.CurrentLimits.StatorCurrentLimit = config.motorConfig().currentLimitAmps() * 2.0;
motorConfig.CurrentLimits.StatorCurrentLimitEnable = true;

motorConfig.CurrentLimits.SupplyCurrentLimit = config.motor.currentLimitAmps();
motorConfig.CurrentLimits.SupplyCurrentLimit = config.motorConfig().currentLimitAmps();
// Allow higher current spikes (150%) for a brief duration (one second)
// REV 40A auto-resetting breakers typically trip when current exceeds 300% for one second
motorConfig.CurrentLimits.SupplyCurrentThreshold = config.motor.currentLimitAmps() * 1.5;
motorConfig.CurrentLimits.SupplyCurrentThreshold =
config.motorConfig().currentLimitAmps() * 1.5;
motorConfig.CurrentLimits.SupplyTimeThreshold = 1;
motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;

motorConfig.Feedback.SensorToMechanismRatio = config.motor.motorToMechanismRatio();
motorConfig.Feedback.SensorToMechanismRatio = config.motorConfig().motorToMechanismRatio();

Configurator.configureTalonFX(leaderMotor.getConfigurator(), motorConfig);
Configurator.configureTalonFX(followerMotor.getConfigurator(), motorConfig);

CANcoderConfiguration encoderConfig = new CANcoderConfiguration();

encoderConfig.MagnetSensor.MagnetOffset = config.absoluteEncoder.offset().getRotations();
encoderConfig.MagnetSensor.MagnetOffset =
config.absoluteEncoderConfig().offset().getRotations();
encoderConfig.MagnetSensor.SensorDirection =
config.absoluteEncoder.ccwPositive()
config.absoluteEncoderConfig().ccwPositive()
? SensorDirectionValue.CounterClockwise_Positive
: SensorDirectionValue.Clockwise_Positive;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,25 +45,26 @@ public void configure() {
TalonFXConfiguration motorConfig = new TalonFXConfiguration();

motorConfig.MotorOutput.Inverted =
config.motor.ccwPositive()
config.motorConfig().ccwPositive()
? InvertedValue.CounterClockwise_Positive
: InvertedValue.Clockwise_Positive;
motorConfig.MotorOutput.NeutralMode =
config.motor.neutralBrake() ? NeutralModeValue.Brake : NeutralModeValue.Coast;
config.motorConfig().neutralBrake() ? NeutralModeValue.Brake : NeutralModeValue.Coast;

// Stator current is a measure of the current inside of the motor and is typically higher than
// supply (breaker) current
motorConfig.CurrentLimits.StatorCurrentLimit = config.motor.currentLimitAmps() * 2.0;
motorConfig.CurrentLimits.StatorCurrentLimit = config.motorConfig().currentLimitAmps() * 2.0;
motorConfig.CurrentLimits.StatorCurrentLimitEnable = true;

motorConfig.CurrentLimits.SupplyCurrentLimit = config.motor.currentLimitAmps();
motorConfig.CurrentLimits.SupplyCurrentLimit = config.motorConfig().currentLimitAmps();
// Allow higher current spikes (150%) for a brief duration (one second)
// REV 40A auto-resetting breakers typically trip when current exceeds 300% for one second
motorConfig.CurrentLimits.SupplyCurrentThreshold = config.motor.currentLimitAmps() * 1.5;
motorConfig.CurrentLimits.SupplyCurrentThreshold =
config.motorConfig().currentLimitAmps() * 1.5;
motorConfig.CurrentLimits.SupplyTimeThreshold = 1;
motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;

motorConfig.Feedback.SensorToMechanismRatio = config.motor.motorToMechanismRatio();
motorConfig.Feedback.SensorToMechanismRatio = config.motorConfig().motorToMechanismRatio();

Configurator.configureTalonFX(motor.getConfigurator(), motorConfig);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,9 @@ public class VelocityControllerIOTalonFXPIDF extends VelocityControllerIOTalonFX
public VelocityControllerIOTalonFXPIDF(CAN can, MechanismConfig config, boolean enableFOC) {
super(can, config);

feedforward = config.feedforward.createSimpleMotorFeedforward();
feedforward = config.feedforwardControllerConfig().createSimpleMotorFeedforward();

feedback = config.feedback.createPIDController();
feedback = config.feedbackControllerConfig().createPIDController();

voltage = new VoltageOut(0.0).withEnableFOC(enableFOC);
}
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/arm/ArmConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -27,22 +27,22 @@ public static class ShoulderConstants {
/** Shoulder's config. */
public static final MechanismConfig CONFIG =
new MechanismConfig()
.withAbsoluteEncoder(
.withAbsoluteEncoderConfig(
new AbsoluteEncoderConfig()
.withCCWPositive(false)
.withOffset(Rotation2d.fromDegrees(-173.135)))
.withMotor(
.withMotorConfig(
new MotorConfig()
.withCCWPositive(true)
.withNeutralBrake(true)
.withMotorToMechanismRatio(39.771428571))
.withFeedforward(
.withFeedforwardConfig(
new FeedforwardControllerConfig()
.withStaticFeedforward(0.14) // volts
.withGravityFeedforward(0.5125) // volts
.withVelocityFeedforward(4.0) // volts per rotation per second
)
.withFeedback(
.withFeedbackConfig(
new FeedbackControllerConfig().withProportionalGain(4.0) // volts per rotation
);

Expand Down
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/intake/IntakeConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -16,17 +16,17 @@ public static class FrontRollerConstants {
/** Front roller's config. */
public static final MechanismConfig CONFIG =
new MechanismConfig()
.withMotor(
.withMotorConfig(
new MotorConfig()
.withCCWPositive(false)
.withNeutralBrake(false)
.withMotorToMechanismRatio(24.0 / 16.0))
.withFeedforward(
.withFeedforwardConfig(
new FeedforwardControllerConfig()
.withStaticFeedforward(0.13) // volts
.withVelocityFeedforward(0.1683) // volts per rotation per second
)
.withFeedback(
.withFeedbackConfig(
new FeedbackControllerConfig()
.withProportionalGain(0.1) // volts per rotation per second
);
Expand All @@ -48,17 +48,17 @@ public static class BackRollerConstants {

public static final MechanismConfig CONFIG =
new MechanismConfig()
.withMotor(
.withMotorConfig(
new MotorConfig()
.withCCWPositive(false)
.withNeutralBrake(false)
.withMotorToMechanismRatio(24.0 / 16.0))
.withFeedforward(
.withFeedforwardConfig(
new FeedforwardControllerConfig()
.withStaticFeedforward(0.13) // volts
.withVelocityFeedforward(0.1759) // volts per rotation per second
)
.withFeedback(
.withFeedbackConfig(
new FeedbackControllerConfig()
.withProportionalGain(0.1) // volts per rotation per second
);
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/shooter/ShooterConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,12 @@ public static class SerializerConstants {
/** Serializer's config. */
public static final MechanismConfig PIDF_CONTROLLER_CONSTANTS =
new MechanismConfig()
.withMotor(
.withMotorConfig(
new MotorConfig()
.withCCWPositive(true)
.withNeutralBrake(false)
.withMotorToMechanismRatio(36.0 / 16.0))
.withFeedforward(
.withFeedforwardConfig(
new FeedforwardControllerConfig()
.withStaticFeedforward(0.14) // volts
.withVelocityFeedforward(0.2617) // volts per rotation per second
Expand Down Expand Up @@ -59,17 +59,17 @@ public static class FlywheelConstants {
/** Flywheel's config. */
public static final MechanismConfig CONFIG =
new MechanismConfig()
.withMotor(
.withMotorConfig(
new MotorConfig()
.withCCWPositive(false)
.withNeutralBrake(true)
.withMotorToMechanismRatio(28.0 / 16.0))
.withFeedforward(
.withFeedforwardConfig(
new FeedforwardControllerConfig()
.withStaticFeedforward(0.14) // volts
.withVelocityFeedforward(0.2) // volts per rotation per second
)
.withFeedback(
.withFeedbackConfig(
new FeedbackControllerConfig()
.withProportionalGain(0.14) // volts per rotation per second
);
Expand Down
7 changes: 5 additions & 2 deletions src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,12 @@ public DriveMotorIOTalonFXPID(CAN can) {
super(can);

velocityFeedforward =
SwerveConstants.DRIVE_PIDF_CONSTANTS.feedforward.createSimpleMotorFeedforward();
SwerveConstants.DRIVE_PIDF_CONSTANTS
.feedforwardControllerConfig()
.createSimpleMotorFeedforward();

velocityFeedback = SwerveConstants.DRIVE_PIDF_CONSTANTS.feedback.createPIDController();
velocityFeedback =
SwerveConstants.DRIVE_PIDF_CONSTANTS.feedbackControllerConfig().createPIDController();

voltageOutRequest = new VoltageOut(0).withEnableFOC(SwerveConstants.USE_PHOENIX_PRO_FOC);
}
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/swerve/SteerMotorIOSim.java
Original file line number Diff line number Diff line change
Expand Up @@ -31,15 +31,15 @@ public SteerMotorIOSim() {
positionRotations = 0.0;
velocityRotationsPerSecond = 0.0;

MechanismConfig pidfConstants = SwerveConstants.STEER_PIDF_CONSTANTS;
MechanismConfig config = SwerveConstants.STEER_PIDF_CONSTANTS;

// Simulation is an ideal environment that does not have friction
pidfConstants =
pidfConstants
.withFeedforward(pidfConstants.feedforward.withStaticFeedforward(0.0))
.withFeedback(pidfConstants.feedback.withPositionTolerance(0.0));
config =
config
.withFeedforwardConfig(config.feedforwardControllerConfig().withStaticFeedforward(0.0))
.withFeedbackConfig(config.feedbackControllerConfig().withPositionTolerance(0.0));

pidf = new SteerMotorPIDF(pidfConstants);
pidf = new SteerMotorPIDF(config);
}

@Override
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/swerve/SteerMotorPIDF.java
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,10 @@ public class SteerMotorPIDF {
private final PIDController feedback;

/** Creates a PIDF utility class. */
public SteerMotorPIDF(MechanismConfig pidfConstants) {
feedforward = pidfConstants.feedforward.createSimpleMotorFeedforward();
public SteerMotorPIDF(MechanismConfig config) {
feedforward = config.feedforwardControllerConfig().createSimpleMotorFeedforward();

feedback = pidfConstants.feedback.createPIDController();
feedback = config.feedbackControllerConfig().createPIDController();
feedback.enableContinuousInput(-0.5, 0.5);
}

Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/swerve/SwerveConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,7 @@ private static double calculateKv(double voltsPerRotorRotationPerSecond) {

public static final MechanismConfig DRIVE_PIDF_CONSTANTS =
new MechanismConfig()
.withFeedforward(
.withFeedforwardConfig(
new FeedforwardControllerConfig()
.withStaticFeedforward(0.14) // volts
.withVelocityFeedforward(calculateKv(0.12)) // volts per meter per second
Expand All @@ -163,10 +163,10 @@ private static double calculateKv(double voltsPerRotorRotationPerSecond) {
/** Constants for steer motor PIDF position controllers. */
public static final MechanismConfig STEER_PIDF_CONSTANTS =
new MechanismConfig()
.withFeedforward(
.withFeedforwardConfig(
new FeedforwardControllerConfig().withStaticFeedforward(0.205) // volts
)
.withFeedback(
.withFeedbackConfig(
new FeedbackControllerConfig()
.withProportionalGain(54.0) // volts per rotation
.withDerivativeGain(0.16) // volts per rotation per second
Expand Down

0 comments on commit 4a21d0c

Please sign in to comment.