diff --git a/src/main/java/frc/lib/config/AbsoluteEncoderConfig.java b/src/main/java/frc/lib/config/AbsoluteEncoderConfig.java index 9c2ede7..707f40e 100644 --- a/src/main/java/frc/lib/config/AbsoluteEncoderConfig.java +++ b/src/main/java/frc/lib/config/AbsoluteEncoderConfig.java @@ -1,8 +1,5 @@ package frc.lib.config; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.geometry.Rotation2d; import java.util.Objects; @@ -63,34 +60,4 @@ public AbsoluteEncoderConfig build() { return new AbsoluteEncoderConfig(ccwPositive, sensorToMechanismRatio, offset); } } - - /** - * Creates Phoenix 6 magnet sensor configs. - * - * @return Phoenix 6 magnet sensor configs. - */ - private MagnetSensorConfigs createMagnetSensorConfigs() { - final MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs(); - - magnetSensorConfigs.MagnetOffset = offset().getRotations(); - magnetSensorConfigs.SensorDirection = - ccwPositive() - ? SensorDirectionValue.CounterClockwise_Positive - : SensorDirectionValue.Clockwise_Positive; - - return magnetSensorConfigs; - } - - /** - * Creates a Phoenix 6 CANcoder config. - * - * @return a Phoenix 6 CANcoder config. - */ - public CANcoderConfiguration createCANcoderConfig() { - final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - - cancoderConfig.MagnetSensor = createMagnetSensorConfigs(); - - return cancoderConfig; - } } diff --git a/src/main/java/frc/lib/config/ConfigApplier.java b/src/main/java/frc/lib/config/ConfigApplier.java deleted file mode 100644 index bdf544f..0000000 --- a/src/main/java/frc/lib/config/ConfigApplier.java +++ /dev/null @@ -1,112 +0,0 @@ -package frc.lib.config; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -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; - -/** Applies configs. */ -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 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 attempt( - Supplier applier, Function isSuccess, int retries) { - for (int i = 0; i < retries; i++) { - Result result = applier.get(); - - if (isSuccess.apply(result)) { - return true; - } - } - - return false; - } - - /** - * Attempts to apply a Phoenix 6 config. Returns true if successful. - * - * @param applier a function that attempts to apply a config. Returns the result of the - * application. - * @return true if successful. - */ - private static boolean attempt(Supplier applier) { - return attempt(() -> applier.get(), StatusCode::isOK, 10); - } - - /** - * Attempts to apply a CANcoder config. Warns on failure. - * - * @param cancoder the CANcoder to configure. - * @param config the config to apply. - * @return true if successful. - */ - 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; - } - - /** - * Attempts to apply a TalonFX config. Warns on failure. - * - * @param talonFX the TalonFX to configure. - * @param config the config to apply. - * @return true if successful. - */ - 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; - } - - /** - * Attempts to apply a Pigeon 2 config. Warns on failure. - * - * @param pigeon2 the Pigeon 2 to configure. - * @param config the config to apply. - * @return true if successful. - */ - 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/config/MotorConfig.java b/src/main/java/frc/lib/config/MotorConfig.java index 9ce52b2..5803e12 100644 --- a/src/main/java/frc/lib/config/MotorConfig.java +++ b/src/main/java/frc/lib/config/MotorConfig.java @@ -1,10 +1,5 @@ package frc.lib.config; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import java.util.Objects; /** Motor config. */ @@ -90,60 +85,4 @@ public MotorConfig build() { neutralBrake, ccwPositive, motorToMechanismRatio, statorCurrentLimit, supplyCurrentLimit); } } - - /** - * Creates Phoenix 6 current limit configs. - * - * @return Phoenix 6 current limit configs. - */ - private CurrentLimitsConfigs createCurrentLimitsConfigs() { - final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); - - // Stator current is a measure of the current inside of the motor and is typically higher than - // supply (breaker) current - currentLimitsConfigs.StatorCurrentLimit = statorCurrentLimit; - currentLimitsConfigs.StatorCurrentLimitEnable = true; - - currentLimitsConfigs.SupplyCurrentLimit = supplyCurrentLimit; - // TODO Determine if spikes should be eliminated to preserve battery - // Allow higher current spikes (150%) for a very brief duration (tenth second) - currentLimitsConfigs.SupplyCurrentThreshold = supplyCurrentLimit * 1.5; - currentLimitsConfigs.SupplyTimeThreshold = 0.1; - currentLimitsConfigs.SupplyCurrentLimitEnable = true; - - return currentLimitsConfigs; - } - - /** - * Creates Phoenix 6 motor output configs. - * - * @return Phoenix 6 motor output configs. - */ - private MotorOutputConfigs createMotorOutputConfigs() { - final MotorOutputConfigs motorOutputConfigs = new MotorOutputConfigs(); - - motorOutputConfigs.Inverted = - ccwPositive() ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive; - motorOutputConfigs.NeutralMode = - neutralBrake() ? NeutralModeValue.Brake : NeutralModeValue.Coast; - - return motorOutputConfigs; - } - - /** - * Creates a Phoenix 6 TalonFX config. - * - * @return a Phoenix 6 TalonFX config. - */ - public TalonFXConfiguration createTalonFXConfig() { - final TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - - talonFXConfig.MotorOutput = createMotorOutputConfigs(); - - talonFXConfig.CurrentLimits = createCurrentLimitsConfigs(); - - talonFXConfig.Feedback.SensorToMechanismRatio = motorToMechanismRatio(); - - return talonFXConfig; - } } diff --git a/src/main/java/frc/lib/config/applier/CANcoderConfigApplier.java b/src/main/java/frc/lib/config/applier/CANcoderConfigApplier.java new file mode 100644 index 0000000..188d3c9 --- /dev/null +++ b/src/main/java/frc/lib/config/applier/CANcoderConfigApplier.java @@ -0,0 +1,73 @@ +package frc.lib.config.applier; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CANcoderConfigurator; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.wpilibj.DriverStation; +import frc.lib.config.AbsoluteEncoderConfig; + +/** Applies CANcoder configs. */ +public class CANcoderConfigApplier extends ConfigApplier { + + /** + * Reports to the user that a CANcoder failed configuration. + * + * @param cancoder the CANcoder. + */ + private static void report(CANcoder cancoder) { + DriverStation.reportWarning( + "Failed to apply config to CANcoder with ID: " + cancoder.getDeviceID(), false); + } + + /** + * Applies a factory default config to a CANcoder. + * + * @param cancoder the CANcoder. + */ + public static void applyFactoryDefault(CANcoder cancoder) { + CANcoderConfiguration factoryDefaults = new CANcoderConfiguration(); + + CANcoderConfigurator configurator = cancoder.getConfigurator(); + + if (attempt(() -> configurator.apply(factoryDefaults)) == false) { + report(cancoder); + } + } + + /** + * Creates the magnet sensor configs for the absolute encoder config. + * + * @param absoluteEncoderConfig the absolute encoder config. + * @return the created magnet sensor configs. + */ + private static MagnetSensorConfigs createMagnetSensorConfigs( + AbsoluteEncoderConfig absoluteEncoderConfig) { + final MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs(); + + magnetSensorConfigs.MagnetOffset = absoluteEncoderConfig.offset().getRotations(); + magnetSensorConfigs.SensorDirection = + absoluteEncoderConfig.ccwPositive() + ? SensorDirectionValue.CounterClockwise_Positive + : SensorDirectionValue.Clockwise_Positive; + + return magnetSensorConfigs; + } + + /** + * Applies an absolute encoder config to a CANcoder. + * + * @param cancoder the CANcoder. + * @param absoluteEncoderConfig the absolute encoder config. + */ + public static void apply(CANcoder cancoder, AbsoluteEncoderConfig absoluteEncoderConfig) { + MagnetSensorConfigs magnetSensorConfigs = createMagnetSensorConfigs(absoluteEncoderConfig); + + CANcoderConfigurator configurator = cancoder.getConfigurator(); + + if (attempt(() -> configurator.apply(magnetSensorConfigs)) == false) { + report(cancoder); + } + } +} diff --git a/src/main/java/frc/lib/config/applier/ConfigApplier.java b/src/main/java/frc/lib/config/applier/ConfigApplier.java new file mode 100644 index 0000000..449743c --- /dev/null +++ b/src/main/java/frc/lib/config/applier/ConfigApplier.java @@ -0,0 +1,42 @@ +package frc.lib.config.applier; + +import com.ctre.phoenix6.StatusCode; +import java.util.function.Function; +import java.util.function.Supplier; + +/** Applies configs. */ +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 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. + */ + protected static boolean attempt( + Supplier applier, Function isSuccess, int retries) { + for (int i = 0; i < retries; i++) { + Result result = applier.get(); + + if (isSuccess.apply(result)) { + return true; + } + } + + return false; + } + + /** + * Attempts to apply a Phoenix 6 config. Returns true if successful. + * + * @param applier a function that attempts to apply a config. Returns the result of the + * application. + * @return true if successful. + */ + protected static boolean attempt(Supplier applier) { + return attempt(() -> applier.get(), StatusCode::isOK, 10); + } +} diff --git a/src/main/java/frc/lib/config/applier/Pigeon2ConfigApplier.java b/src/main/java/frc/lib/config/applier/Pigeon2ConfigApplier.java new file mode 100644 index 0000000..1c7efbe --- /dev/null +++ b/src/main/java/frc/lib/config/applier/Pigeon2ConfigApplier.java @@ -0,0 +1,35 @@ +package frc.lib.config.applier; + +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Pigeon2Configurator; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.wpilibj.DriverStation; + +/** Applies Pigeon 2 configs. */ +public class Pigeon2ConfigApplier extends ConfigApplier { + + /** + * Reports to the user that a Pigeon 2 failed configuration. + * + * @param pigeon2 the Pigeon 2. + */ + private static void report(Pigeon2 pigeon2) { + DriverStation.reportWarning( + "Failed to apply config to Pigeon 2 with ID: " + pigeon2.getDeviceID(), false); + } + + /** + * Applies a factory default config to a Pigeon 2. + * + * @param pigeon2 the Pigeon 2. + */ + public static void applyFactoryDefault(Pigeon2 pigeon2) { + Pigeon2Configuration factoryDefaults = new Pigeon2Configuration(); + + Pigeon2Configurator configurator = pigeon2.getConfigurator(); + + if (attempt(() -> configurator.apply(factoryDefaults)) == false) { + report(pigeon2); + } + } +} diff --git a/src/main/java/frc/lib/config/applier/TalonFXConfigApplier.java b/src/main/java/frc/lib/config/applier/TalonFXConfigApplier.java new file mode 100644 index 0000000..510b12f --- /dev/null +++ b/src/main/java/frc/lib/config/applier/TalonFXConfigApplier.java @@ -0,0 +1,126 @@ +package frc.lib.config.applier; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.DriverStation; +import frc.lib.config.MotorConfig; + +/** Applies TalonFX configs. */ +public class TalonFXConfigApplier extends ConfigApplier { + + /** + * Reports to the user that a TalonFX failed configuration. + * + * @param talonFX the TalonFX. + */ + private static void report(TalonFX talonFX) { + DriverStation.reportWarning( + "Failed to apply config to TalonFX with ID: " + talonFX.getDeviceID(), false); + } + + /** + * Applies a factory default config to a TalonFX. + * + * @param talonFX the TalonFX. + */ + public static void applyFactoryDefault(TalonFX talonFX) { + TalonFXConfiguration factoryDefaults = new TalonFXConfiguration(); + + TalonFXConfigurator configurator = talonFX.getConfigurator(); + + if (attempt(() -> configurator.apply(factoryDefaults)) == false) { + report(talonFX); + } + } + + /*** + * Creates the current limits configs for the motor config. + * + * @param motorConfig the motor config. + * @return the created current limits configs. + */ + private static CurrentLimitsConfigs createCurrentLimitsConfigs(MotorConfig motorConfig) { + CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); + + currentLimitsConfigs.StatorCurrentLimit = motorConfig.statorCurrentLimit(); + currentLimitsConfigs.StatorCurrentLimitEnable = true; + + currentLimitsConfigs.SupplyCurrentLimit = motorConfig.supplyCurrentLimit(); + + // TODO Determine if spikes should be further reduced to preserve battery + // Allows higher current spikes for a very brief duration + currentLimitsConfigs.SupplyCurrentThreshold = motorConfig.supplyCurrentLimit() * 1.5; + currentLimitsConfigs.SupplyTimeThreshold = 0.1; + + currentLimitsConfigs.SupplyCurrentLimitEnable = true; + + return currentLimitsConfigs; + } + + /** + * Creates the feedback configs for the motor config. + * + * @param motorConfig the motor config. + * @return the created feedback configs. + */ + private static FeedbackConfigs createFeedbackConfigs(MotorConfig motorConfig) { + FeedbackConfigs feedbackConfigs = new FeedbackConfigs(); + + // TODO This needs to be rethought if a fused/remote sensor is going to be used + // By default, the sensor is the motor sensor, so we can use the motor sensor + feedbackConfigs.SensorToMechanismRatio = motorConfig.motorToMechanismRatio(); + + return feedbackConfigs; + } + + /** + * Creates the motor output configs for the motor config. + * + * @param motorConfig the motor config. + * @return the created motor output configs. + */ + private static MotorOutputConfigs createMotorOutputConfigs(MotorConfig motorConfig) { + MotorOutputConfigs motorOutputConfigs = new MotorOutputConfigs(); + + motorOutputConfigs.Inverted = + motorConfig.ccwPositive() + ? InvertedValue.CounterClockwise_Positive + : InvertedValue.Clockwise_Positive; + motorOutputConfigs.NeutralMode = + motorConfig.neutralBrake() ? NeutralModeValue.Brake : NeutralModeValue.Coast; + + return motorOutputConfigs; + } + + /** + * Applies a motor config to a TalonFX. + * + * @param talonFX the TalonFX. + * @param motorConfig the motor config. + */ + public static void apply(TalonFX talonFX, MotorConfig motorConfig) { + CurrentLimitsConfigs currentLimitsConfigs = createCurrentLimitsConfigs(motorConfig); + FeedbackConfigs feedbackConfigs = createFeedbackConfigs(motorConfig); + MotorOutputConfigs motorOutputConfigs = createMotorOutputConfigs(motorConfig); + + TalonFXConfigurator configurator = talonFX.getConfigurator(); + + if (attempt(() -> configurator.apply(currentLimitsConfigs)) == false) { + report(talonFX); + } + + if (attempt(() -> configurator.apply(feedbackConfigs)) == false) { + report(talonFX); + } + + if (attempt(() -> configurator.apply(motorOutputConfigs)) == false) { + report(talonFX); + } + } +} diff --git a/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java b/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java index 8bb9947..b7ef3cf 100644 --- a/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java +++ b/src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java @@ -11,8 +11,9 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; import frc.lib.CAN; -import frc.lib.config.ConfigApplier; import frc.lib.config.MechanismConfig; +import frc.lib.config.applier.CANcoderConfigApplier; +import frc.lib.config.applier.TalonFXConfigApplier; /** Position controller using two TalonFXs and a CANcoder and an external PIDF for an arm. */ public class PositionControllerIOTalonFX2 implements PositionControllerIO { @@ -79,11 +80,14 @@ public void configure() { ParentDevice.optimizeBusUtilizationForAll(leaderMotor, followerMotor, encoder); - ConfigApplier.applyTalonFXConfig(leaderMotor, config.motorConfig().createTalonFXConfig()); - ConfigApplier.applyTalonFXConfig(followerMotor, config.motorConfig().createTalonFXConfig()); + TalonFXConfigApplier.applyFactoryDefault(leaderMotor); + TalonFXConfigApplier.apply(leaderMotor, config.motorConfig()); - ConfigApplier.applyCANcoderConfig( - encoder, config.absoluteEncoderConfig().createCANcoderConfig()); + TalonFXConfigApplier.applyFactoryDefault(followerMotor); + TalonFXConfigApplier.apply(followerMotor, config.motorConfig()); + + CANcoderConfigApplier.applyFactoryDefault(encoder); + CANcoderConfigApplier.apply(encoder, config.absoluteEncoderConfig()); } @Override diff --git a/src/main/java/frc/lib/controller/PositionControllerIOTalonFXSteer.java b/src/main/java/frc/lib/controller/PositionControllerIOTalonFXSteer.java index b1191e9..bb66129 100644 --- a/src/main/java/frc/lib/controller/PositionControllerIOTalonFXSteer.java +++ b/src/main/java/frc/lib/controller/PositionControllerIOTalonFXSteer.java @@ -9,17 +9,18 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import frc.lib.CAN; -import frc.lib.config.ConfigApplier; import frc.lib.config.MechanismConfig; +import frc.lib.config.applier.CANcoderConfigApplier; +import frc.lib.config.applier.TalonFXConfigApplier; /** Creates a new position controller using a steer TalonFX and azimuth CANcoder. */ public class PositionControllerIOTalonFXSteer implements PositionControllerIO { private final MechanismConfig config; - private final TalonFX steer; + private final TalonFX steerMotor; - private final CANcoder azimuth; + private final CANcoder azimuthEncoder; private final StatusSignal position, velocity, acceleration, volts, amps; @@ -33,18 +34,18 @@ public PositionControllerIOTalonFXSteer( CAN steerCAN, CAN encoderCAN, MechanismConfig config, boolean enableFOC) { this.config = config; - steer = new TalonFX(steerCAN.id(), steerCAN.bus()); + steerMotor = new TalonFX(steerCAN.id(), steerCAN.bus()); - azimuth = new CANcoder(encoderCAN.id(), encoderCAN.bus()); + azimuthEncoder = new CANcoder(encoderCAN.id(), encoderCAN.bus()); - // TODO Use steer position after seeding with azimuth - position = azimuth.getAbsolutePosition(); + // TODO Use steer position after seeding with azimuth to be more resistant to failure + position = azimuthEncoder.getAbsolutePosition(); - velocity = steer.getVelocity(); - acceleration = steer.getAcceleration(); + velocity = steerMotor.getVelocity(); + acceleration = steerMotor.getAcceleration(); - volts = steer.getMotorVoltage(); - amps = steer.getStatorCurrent(); + volts = steerMotor.getMotorVoltage(); + amps = steerMotor.getStatorCurrent(); feedforward = config.feedforwardControllerConfig().createSimpleMotorFeedforward(); @@ -57,12 +58,12 @@ public PositionControllerIOTalonFXSteer( public void configure() { BaseStatusSignal.setUpdateFrequencyForAll(100.0, position, velocity, acceleration, volts, amps); - ParentDevice.optimizeBusUtilizationForAll(steer, azimuth); + ParentDevice.optimizeBusUtilizationForAll(steerMotor, azimuthEncoder); - ConfigApplier.applyTalonFXConfig(steer, config.motorConfig().createTalonFXConfig()); + TalonFXConfigApplier.apply(steerMotor, config.motorConfig()); - ConfigApplier.applyCANcoderConfig( - azimuth, config.absoluteEncoderConfig().createCANcoderConfig()); + CANcoderConfigApplier.applyFactoryDefault(azimuthEncoder); + CANcoderConfigApplier.apply(azimuthEncoder, config.absoluteEncoderConfig()); } @Override @@ -88,7 +89,7 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); - steer.setControl(voltage.withOutput(feedforwardVolts + feedbackVolts)); + steerMotor.setControl(voltage.withOutput(feedforwardVolts + feedbackVolts)); } private double calculateFeedforward(double measurementRotations, double setpointRotations) { diff --git a/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java index 2c5db4d..dad1e24 100644 --- a/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java +++ b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java @@ -5,8 +5,8 @@ import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; import frc.lib.CAN; -import frc.lib.config.ConfigApplier; import frc.lib.config.MechanismConfig; +import frc.lib.config.applier.TalonFXConfigApplier; /** Velocity controller using TalonFX. */ public abstract class VelocityControllerIOTalonFX implements VelocityControllerIO { @@ -40,7 +40,7 @@ public void configure() { ParentDevice.optimizeBusUtilizationForAll(motor); - ConfigApplier.applyTalonFXConfig(motor, config.motorConfig().createTalonFXConfig()); + TalonFXConfigApplier.apply(motor, config.motorConfig()); } @Override diff --git a/src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java b/src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java index 5f5f0aa..04d667e 100644 --- a/src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java +++ b/src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java @@ -2,10 +2,9 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.util.Units; -import frc.lib.config.ConfigApplier; +import frc.lib.config.applier.Pigeon2ConfigApplier; /** Pigeon 2 gyroscope. */ public class GyroscopeIOPigeon2 implements GyroscopeIO { @@ -30,7 +29,7 @@ public GyroscopeIOPigeon2() { @Override public void configure() { - ConfigApplier.applyPigeon2Config(gyroscope, new Pigeon2Configuration()); + Pigeon2ConfigApplier.applyFactoryDefault(gyroscope); } @Override