diff --git a/src/main/java/frc/lib/config/ConfigApplier.java b/src/main/java/frc/lib/config/ConfigApplier.java index 89568d6..bdf544f 100644 --- a/src/main/java/frc/lib/config/ConfigApplier.java +++ b/src/main/java/frc/lib/config/ConfigApplier.java @@ -7,6 +7,10 @@ import com.ctre.phoenix6.configs.Pigeon2Configurator; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.DriverStation; import java.util.function.Function; import java.util.function.Supplier; @@ -15,13 +19,15 @@ public class ConfigApplier { /** * Attempts to apply a config. Returns true if successful. - * - * @param applier a function that attempts to apply a config. Returns the result of the application. + * + * @param applier a function that attempts to apply a config. Returns the result of the + * application. * @param isSuccess a function that returns true if the result of an application is a success. * @param retries the number of unsuccessful attempts before failing. * @return true if successful. */ - private static boolean apply(Supplier applier, Function isSuccess, int retries) { + private static boolean attempt( + Supplier applier, Function isSuccess, int retries) { for (int i = 0; i < retries; i++) { Result result = applier.get(); @@ -35,45 +41,72 @@ private static boolean apply(Supplier applier, Function applier) { - return apply(() -> applier.get(), StatusCode::isOK, 10); + private static boolean attempt(Supplier applier) { + return attempt(() -> applier.get(), StatusCode::isOK, 10); } /** - * Configures a CANcoder. + * Attempts to apply a CANcoder config. Warns on failure. * - * @param configurator the CANcoder's configurator. + * @param cancoder the CANcoder to configure. * @param config the config to apply. + * @return true if successful. */ - public static void configureCANcoder( - CANcoderConfigurator configurator, CANcoderConfiguration config) { - apply(() -> configurator.apply(config)); + public static boolean applyCANcoderConfig(CANcoder cancoder, CANcoderConfiguration config) { + CANcoderConfigurator configurator = cancoder.getConfigurator(); + + boolean success = attempt(() -> configurator.apply(config)); + + if (!success) { + DriverStation.reportWarning( + "Failed to apply config for CANcoder ID: " + cancoder.getDeviceID(), false); + } + + return success; } /** - * Configures a TalonFX. + * Attempts to apply a TalonFX config. Warns on failure. * - * @param configurator the TalonFX's configurator. + * @param talonFX the TalonFX to configure. * @param config the config to apply. + * @return true if successful. */ - public static void configureTalonFX( - TalonFXConfigurator configurator, TalonFXConfiguration config) { - apply(() -> configurator.apply(config)); + public static boolean applyTalonFXConfig(TalonFX talonFX, TalonFXConfiguration config) { + TalonFXConfigurator configurator = talonFX.getConfigurator(); + + boolean success = attempt(() -> configurator.apply(config)); + + if (!success) { + DriverStation.reportWarning( + "Failed to apply config for TalonFX ID: " + talonFX.getDeviceID(), false); + } + + return success; } /** - * Configures a Pigeon 2. + * Attempts to apply a Pigeon 2 config. Warns on failure. * - * @param configurator the Pigeon 2's configurator. + * @param pigeon2 the Pigeon 2 to configure. * @param config the config to apply. + * @return true if successful. */ - public static void configurePigeon2( - Pigeon2Configurator configurator, Pigeon2Configuration config) { - apply(() -> configurator.apply(config)); - } + public static boolean applyPigeon2Config(Pigeon2 pigeon2, Pigeon2Configuration config) { + Pigeon2Configurator configurator = pigeon2.getConfigurator(); + boolean success = attempt(() -> configurator.apply(config)); + + if (!success) { + DriverStation.reportWarning( + "Failed to apply config for Pigeon 2 ID: " + pigeon2.getDeviceID(), false); + } + + return success; + } } diff --git a/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java b/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java index 00adb7e..8bb9947 100644 --- a/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java +++ b/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java @@ -79,13 +79,11 @@ public void configure() { ParentDevice.optimizeBusUtilizationForAll(leaderMotor, followerMotor, encoder); - ConfigApplier.configureTalonFX( - leaderMotor.getConfigurator(), config.motorConfig().createTalonFXConfig()); - ConfigApplier.configureTalonFX( - followerMotor.getConfigurator(), config.motorConfig().createTalonFXConfig()); + ConfigApplier.applyTalonFXConfig(leaderMotor, config.motorConfig().createTalonFXConfig()); + ConfigApplier.applyTalonFXConfig(followerMotor, config.motorConfig().createTalonFXConfig()); - ConfigApplier.configureCANcoder( - encoder.getConfigurator(), config.absoluteEncoderConfig().createCANcoderConfig()); + ConfigApplier.applyCANcoderConfig( + encoder, config.absoluteEncoderConfig().createCANcoderConfig()); } @Override diff --git a/src/main/java/frc/lib/controller/PositionControllerIOTalonFXSteer.java b/src/main/java/frc/lib/controller/PositionControllerIOTalonFXSteer.java index 244bf84..b1191e9 100644 --- a/src/main/java/frc/lib/controller/PositionControllerIOTalonFXSteer.java +++ b/src/main/java/frc/lib/controller/PositionControllerIOTalonFXSteer.java @@ -59,11 +59,10 @@ public void configure() { ParentDevice.optimizeBusUtilizationForAll(steer, azimuth); - ConfigApplier.configureTalonFX( - steer.getConfigurator(), config.motorConfig().createTalonFXConfig()); + ConfigApplier.applyTalonFXConfig(steer, config.motorConfig().createTalonFXConfig()); - ConfigApplier.configureCANcoder( - azimuth.getConfigurator(), config.absoluteEncoderConfig().createCANcoderConfig()); + ConfigApplier.applyCANcoderConfig( + azimuth, config.absoluteEncoderConfig().createCANcoderConfig()); } @Override diff --git a/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java index 4b609a9..2c5db4d 100644 --- a/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java +++ b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java @@ -40,8 +40,7 @@ public void configure() { ParentDevice.optimizeBusUtilizationForAll(motor); - ConfigApplier.configureTalonFX( - motor.getConfigurator(), config.motorConfig().createTalonFXConfig()); + ConfigApplier.applyTalonFXConfig(motor, config.motorConfig().createTalonFXConfig()); } @Override diff --git a/src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java b/src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java index a37c11c..5f5f0aa 100644 --- a/src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java +++ b/src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java @@ -30,7 +30,7 @@ public GyroscopeIOPigeon2() { @Override public void configure() { - ConfigApplier.configurePigeon2(gyroscope.getConfigurator(), new Pigeon2Configuration()); + ConfigApplier.applyPigeon2Config(gyroscope, new Pigeon2Configuration()); } @Override