From 855010bb260483930aee831df048596b0dd6925b Mon Sep 17 00:00:00 2001 From: Ishan1522 Date: Tue, 20 Aug 2024 17:15:14 -0400 Subject: [PATCH] remove talonutil for now --- src/main/java/frc/robot/extras/TalonUtil.java | 87 ------------------- .../subsystems/shooter/ShooterSubsystem.java | 19 ++-- 2 files changed, 6 insertions(+), 100 deletions(-) delete mode 100644 src/main/java/frc/robot/extras/TalonUtil.java diff --git a/src/main/java/frc/robot/extras/TalonUtil.java b/src/main/java/frc/robot/extras/TalonUtil.java deleted file mode 100644 index f3c6314..0000000 --- a/src/main/java/frc/robot/extras/TalonUtil.java +++ /dev/null @@ -1,87 +0,0 @@ -// moderately inspired by 254 -package frc.robot.extras; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj.DriverStation; -import java.util.function.Supplier; - -public class TalonUtil { - - public static boolean applyAndCheckConfiguration( - TalonFX talon, TalonFXConfiguration config, double timeoutSeconds, int numTries) { - for (int i = 0; i < numTries; i++) { - if (checkErrorAndRetry(() -> talon.getConfigurator().apply(config, timeoutSeconds))) { - return true; - } - } - DriverStation.reportError( - "Failed to apply config for talon after " + numTries + " attempts", false); - return false; - } - - public static boolean applyAndCheckConfiguration( - TalonFX talon, TalonFXConfiguration config, double timeoutMs) { - boolean result = applyAndCheckConfiguration(talon, config, timeoutMs, 5); - return result; - } - - public enum StickyFault { - BootDuringEnable, - DeviceTemp, - ForwardHardLimit, - ForwardSoftLimit, - Hardware, - OverSupplyV, - ProcTemp, - ReverseHardLimit, - ReverseSoftLimit, - Undervoltage, - UnstableSupplyV - } - - public static void checkStickyFaults(String motorName, TalonFX talon) { - boolean[] faults = new boolean[StickyFault.values().length]; - faults[0] = talon.getStickyFault_BootDuringEnable().getValue(); - faults[1] = talon.getStickyFault_DeviceTemp().getValue(); - faults[2] = talon.getStickyFault_ForwardHardLimit().getValue(); - faults[3] = talon.getStickyFault_ForwardSoftLimit().getValue(); - faults[4] = talon.getStickyFault_Hardware().getValue(); - faults[5] = talon.getStickyFault_OverSupplyV().getValue(); - faults[6] = talon.getStickyFault_ProcTemp().getValue(); - faults[7] = talon.getStickyFault_ReverseHardLimit().getValue(); - faults[8] = talon.getStickyFault_ReverseSoftLimit().getValue(); - faults[9] = talon.getStickyFault_Undervoltage().getValue(); - faults[10] = talon.getStickyFault_UnstableSupplyV().getValue(); - - for (int i = 0; i < faults.length; i++) { - if (faults[i]) { - DriverStation.reportError( - motorName + ": Talon Fault! " + StickyFault.values()[i].toString(), false); - } - } - - talon.clearStickyFaults(); - } - - public static boolean checkErrorAndRetry(Supplier function, int numTries) { - StatusCode code = function.get(); - int tries = 0; - while (code != StatusCode.OK && tries < numTries) { - DriverStation.reportWarning("Retrying CTRE Device Config " + code.getName(), false); - code = function.get(); - tries++; - } - if (code != StatusCode.OK) { - DriverStation.reportError( - "Failed to execute phoenix pro api call after " + numTries + " attempts", false); - return false; - } - return true; - } - - public static boolean checkErrorAndRetry(Supplier function) { - return checkErrorAndRetry(function, 5); - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index d37ebfd..1d13ab6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -16,7 +16,6 @@ import frc.robot.Constants.HardwareConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.extras.SmarterDashboardRegistry; -import frc.robot.extras.TalonUtil; public class ShooterSubsystem extends SubsystemBase { private final TalonFX leaderFlywheel; @@ -59,11 +58,8 @@ public ShooterSubsystem() { private void configureTalons() throws RuntimeException { TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); - TalonUtil.checkErrorAndRetry( - () -> leaderFlywheel.getConfigurator().refresh(shooterConfig, HardwareConstants.TIMEOUT_S)); - TalonUtil.checkErrorAndRetry( - () -> - followerFlywheel.getConfigurator().refresh(shooterConfig, HardwareConstants.TIMEOUT_S)); + leaderFlywheel.getConfigurator().refresh(shooterConfig, HardwareConstants.TIMEOUT_S); + followerFlywheel.getConfigurator().refresh(shooterConfig, HardwareConstants.TIMEOUT_S); shooterConfig.Slot0.kP = ShooterConstants.SHOOT_P; shooterConfig.Slot0.kI = ShooterConstants.SHOOT_I; @@ -80,19 +76,16 @@ private void configureTalons() throws RuntimeException { shooterConfig.CurrentLimits.SupplyCurrentLimit = ShooterConstants.SHOOTER_SUPPLY_LIMIT; shooterConfig.CurrentLimits.SupplyCurrentLimitEnable = ShooterConstants.SHOOTER_SUPPLY_ENABLE; - TalonUtil.applyAndCheckConfiguration( - leaderFlywheel, shooterConfig, HardwareConstants.TIMEOUT_S); + leaderFlywheel.getConfigurator().apply(shooterConfig, HardwareConstants.TIMEOUT_S); shooterConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - TalonUtil.applyAndCheckConfiguration( - followerFlywheel, shooterConfig, HardwareConstants.TIMEOUT_S); + followerFlywheel.getConfigurator().apply(shooterConfig, HardwareConstants.TIMEOUT_S); TalonFXConfiguration rollerConfig = new TalonFXConfiguration(); - TalonUtil.checkErrorAndRetry( - () -> leaderFlywheel.getConfigurator().refresh(rollerConfig, HardwareConstants.TIMEOUT_S)); + rollerMotor.getConfigurator().refresh(rollerConfig, HardwareConstants.TIMEOUT_S); rollerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; rollerConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND; - TalonUtil.applyAndCheckConfiguration(rollerMotor, rollerConfig, HardwareConstants.TIMEOUT_S); + rollerMotor.getConfigurator().apply(rollerConfig, HardwareConstants.TIMEOUT_S); } /**