From a240c1f09f5f1d9483de3c1d8ddcd25e8ef81e4f Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Tue, 23 Jan 2024 01:01:33 -0500 Subject: [PATCH] refactor: Use lambdas for configuration. --- .../{ConfigApplier.java => Configurator.java} | 53 +++++++++++++------ .../robot/intake/IntakeMotorIOSparkMax.java | 7 ++- .../robot/odometry/GyroscopeIOPigeon2.java | 4 +- .../swerve/AzimuthEncoderIOCANcoder.java | 4 +- .../robot/swerve/DriveMotorIOTalonFXPID.java | 4 +- .../swerve/SteerMotorIOTalonFXIntegrated.java | 4 +- .../robot/swerve/SteerMotorIOTalonFXPIDF.java | 4 +- 7 files changed, 51 insertions(+), 29 deletions(-) rename src/main/java/frc/lib/{ConfigApplier.java => Configurator.java} (55%) diff --git a/src/main/java/frc/lib/ConfigApplier.java b/src/main/java/frc/lib/Configurator.java similarity index 55% rename from src/main/java/frc/lib/ConfigApplier.java rename to src/main/java/frc/lib/Configurator.java index 5493902..e5bb469 100644 --- a/src/main/java/frc/lib/ConfigApplier.java +++ b/src/main/java/frc/lib/Configurator.java @@ -7,26 +7,40 @@ import com.ctre.phoenix6.configs.Pigeon2Configurator; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.revrobotics.REVLibError; +import java.util.function.Supplier; /** Applies hardware configurations. */ -public class ConfigApplier { +public class Configurator { /** Number of retries for configuring Phoenix hardware. */ private static final int PHOENIX_RETRY_COUNT = 10; + /** Number of retries for configuring REV hardware. */ + private static final int REV_RETRY_COUNT = 10; + + /** + * Configures a Phoenix device. + * + * @param config a lambda that configures a Phoenix device. + */ + public static void configurePhoenix(Supplier config) { + for (int i = 0; i < PHOENIX_RETRY_COUNT; i++) { + if (config.get() == StatusCode.OK) { + return; + } + } + } + /** * Configures a CANcoder. * * @param configurator the CANcoder's configurator. * @param config the config to apply. */ - public static void applyCANcoderConfig( + public static void configureCANcoder( CANcoderConfigurator configurator, CANcoderConfiguration config) { - for (int i = 0; i < PHOENIX_RETRY_COUNT; i++) { - if (configurator.apply(config) == StatusCode.OK) { - break; - } - } + configurePhoenix(() -> configurator.apply(config)); } /** @@ -35,13 +49,9 @@ public static void applyCANcoderConfig( * @param configurator the TalonFX's configurator. * @param config the config to apply. */ - public static void applyTalonFXConfig( + public static void configureTalonFX( TalonFXConfigurator configurator, TalonFXConfiguration config) { - for (int i = 0; i < PHOENIX_RETRY_COUNT; i++) { - if (configurator.apply(config) == StatusCode.OK) { - break; - } - } + configurePhoenix(() -> configurator.apply(config)); } /** @@ -50,11 +60,20 @@ public static void applyTalonFXConfig( * @param configurator the Pigeon 2's configurator. * @param config the config to apply. */ - public static void applyPigeon2Config( + public static void configurePigeon2( Pigeon2Configurator configurator, Pigeon2Configuration config) { - for (int i = 0; i < PHOENIX_RETRY_COUNT; i++) { - if (configurator.apply(config) == StatusCode.OK) { - break; + configurePhoenix(() -> configurator.apply(config)); + } + + /** + * Configures a REV device. + * + * @param config a lambda that configures a REV device. + */ + public static void configureREV(Supplier config) { + for (int i = 0; i < REV_RETRY_COUNT; i++) { + if (config.get() == REVLibError.kOk) { + return; } } } diff --git a/src/main/java/frc/robot/intake/IntakeMotorIOSparkMax.java b/src/main/java/frc/robot/intake/IntakeMotorIOSparkMax.java index 12c6e9f..8fac9c4 100644 --- a/src/main/java/frc/robot/intake/IntakeMotorIOSparkMax.java +++ b/src/main/java/frc/robot/intake/IntakeMotorIOSparkMax.java @@ -2,6 +2,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import frc.lib.Configurator; import frc.robot.intake.IntakeConstants.IntakeMotorConstants; /** Intake motor using a Spark Max. */ @@ -16,11 +17,13 @@ public IntakeMotorIOSparkMax() { @Override public void configure() { - sparkMax.restoreFactoryDefaults(); + Configurator.configureREV(sparkMax::restoreFactoryDefaults); sparkMax.setInverted(IntakeMotorConstants.IS_INVERTED); - sparkMax.setSmartCurrentLimit((int) IntakeMotorConstants.CURRENT_LIMITS.breakerAmps()); + Configurator.configureREV( + () -> + sparkMax.setSmartCurrentLimit((int) IntakeMotorConstants.CURRENT_LIMITS.breakerAmps())); } @Override diff --git a/src/main/java/frc/robot/odometry/GyroscopeIOPigeon2.java b/src/main/java/frc/robot/odometry/GyroscopeIOPigeon2.java index 6a84558..89c489a 100644 --- a/src/main/java/frc/robot/odometry/GyroscopeIOPigeon2.java +++ b/src/main/java/frc/robot/odometry/GyroscopeIOPigeon2.java @@ -5,7 +5,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.util.Units; import frc.lib.CAN; -import frc.lib.ConfigApplier; +import frc.lib.Configurator; /** Pigeon 2 gyroscope. */ public class GyroscopeIOPigeon2 implements GyroscopeIO { @@ -33,7 +33,7 @@ public GyroscopeIOPigeon2(CAN gyroscopeCAN) { public void configure() { Pigeon2Configuration config = OdometryFactory.createGyroscopeConfig(); - ConfigApplier.applyPigeon2Config(pigeon2.getConfigurator(), config); + Configurator.configurePigeon2(pigeon2.getConfigurator(), config); } @Override diff --git a/src/main/java/frc/robot/swerve/AzimuthEncoderIOCANcoder.java b/src/main/java/frc/robot/swerve/AzimuthEncoderIOCANcoder.java index e6bdb45..68b760d 100644 --- a/src/main/java/frc/robot/swerve/AzimuthEncoderIOCANcoder.java +++ b/src/main/java/frc/robot/swerve/AzimuthEncoderIOCANcoder.java @@ -5,7 +5,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.math.geometry.Rotation2d; import frc.lib.CAN; -import frc.lib.ConfigApplier; +import frc.lib.Configurator; /** CANcoder azimuth encoder. */ public class AzimuthEncoderIOCANcoder implements AzimuthEncoderIO { @@ -39,7 +39,7 @@ public void configure() { config.MagnetSensor.MagnetOffset = magnetOffset.getRotations(); - ConfigApplier.applyCANcoderConfig(cancoder.getConfigurator(), config); + Configurator.configureCANcoder(cancoder.getConfigurator(), config); } @Override diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java index 65c64a4..affd36d 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import frc.lib.CAN; -import frc.lib.ConfigApplier; +import frc.lib.Configurator; import frc.robot.RobotConstants; import frc.robot.swerve.SwerveConstants.DriveMotorConstants; import frc.robot.swerve.SwerveConstants.MK4iConstants; @@ -35,7 +35,7 @@ public DriveMotorIOTalonFXPID(CAN can) { public void configure() { TalonFXConfiguration config = SwerveFactory.createDriveMotorConfig(); - ConfigApplier.applyTalonFXConfig(talonFX.getConfigurator(), config); + Configurator.configureTalonFX(talonFX.getConfigurator(), config); } @Override diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java index 58a81c4..0363899 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java @@ -3,7 +3,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import frc.lib.CAN; -import frc.lib.ConfigApplier; +import frc.lib.Configurator; import frc.robot.swerve.SwerveConstants.MK4iConstants; /** TalonFX steer motor controlled by an internal controller. */ @@ -28,7 +28,7 @@ public void configure() { config.MotionMagic.MotionMagicCruiseVelocity = 100.0 / MK4iConstants.STEER_GEARING; config.MotionMagic.MotionMagicAcceleration = config.MotionMagic.MotionMagicCruiseVelocity * 10; - ConfigApplier.applyTalonFXConfig(talonFX.getConfigurator(), config); + Configurator.configureTalonFX(talonFX.getConfigurator(), config); } @Override diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java index 7a971ab..fae6e4c 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java @@ -5,7 +5,7 @@ import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Rotation2d; import frc.lib.CAN; -import frc.lib.ConfigApplier; +import frc.lib.Configurator; import frc.robot.swerve.SwerveConstants.MK4iConstants; /** TalonFX steer motor controlled by an external PIDF controller. */ @@ -33,7 +33,7 @@ public void configure() { config.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING; - ConfigApplier.applyTalonFXConfig(talonFX.getConfigurator(), config); + Configurator.configureTalonFX(talonFX.getConfigurator(), config); } @Override