From c5164ed7a8ddcd62d41ebebda2923c43dc489d29 Mon Sep 17 00:00:00 2001 From: Jonah Snider Date: Mon, 15 Jul 2024 18:31:05 -0700 Subject: [PATCH] Replace custom swerve current limit config code with new Phoenix initialConfigs feature --- .../java/frc/robot/config/CompConfig.java | 18 +------------- .../java/frc/robot/config/PracticeConfig.java | 19 +-------------- .../java/frc/robot/config/RobotConfig.java | 5 ---- .../generated/CompBotTunerConstants.java | 23 ++++++++++++++++++ .../generated/PracticeBotTunerConstants.java | 23 ++++++++++++++++++ .../frc/robot/swerve/SwerveSubsystem.java | 24 ------------------- 6 files changed, 48 insertions(+), 64 deletions(-) diff --git a/src/main/java/frc/robot/config/CompConfig.java b/src/main/java/frc/robot/config/CompConfig.java index 11fc96c3..f98e349c 100644 --- a/src/main/java/frc/robot/config/CompConfig.java +++ b/src/main/java/frc/robot/config/CompConfig.java @@ -252,24 +252,8 @@ class CompConfig { .withClosedLoopRamps(CLOSED_LOOP_RAMP) .withOpenLoopRamps(OPEN_LOOP_RAMP)), new SwerveConfig( - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(20) - .withStatorCurrentLimit(70) - .withSupplyCurrentLimitEnable(true) - .withStatorCurrentLimitEnable(true), - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(120) - .withStatorCurrentLimit(70) - .withSupplyCurrentLimitEnable(true) - .withStatorCurrentLimitEnable(true), - new TorqueCurrentConfigs() - .withPeakForwardTorqueCurrent(80) - .withPeakReverseTorqueCurrent(-80), // new PhoenixPIDController(50, 0, 5), - new PhoenixPIDController(20, 0, 2), - true, - true, - true), + new PhoenixPIDController(20, 0, 2), true, true, true), new IMUConfig( 1, distanceToAngleTolerance -> { diff --git a/src/main/java/frc/robot/config/PracticeConfig.java b/src/main/java/frc/robot/config/PracticeConfig.java index 44ea4ea4..cdde9cfb 100644 --- a/src/main/java/frc/robot/config/PracticeConfig.java +++ b/src/main/java/frc/robot/config/PracticeConfig.java @@ -247,24 +247,7 @@ class PracticeConfig { new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withClosedLoopRamps(CLOSED_LOOP_RAMP) .withOpenLoopRamps(OPEN_LOOP_RAMP)), - new SwerveConfig( - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(20) - .withStatorCurrentLimit(70) - .withSupplyCurrentLimitEnable(true) - .withStatorCurrentLimitEnable(true), - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(150) - .withStatorCurrentLimit(70) - .withSupplyCurrentLimitEnable(true) - .withStatorCurrentLimitEnable(true), - new TorqueCurrentConfigs() - .withPeakForwardTorqueCurrent(80) - .withPeakReverseTorqueCurrent(-80), - new PhoenixPIDController(10, 0, 1), - true, - true, - true), + new SwerveConfig(new PhoenixPIDController(10, 0, 1), true, true, true), new IMUConfig( 1, distanceToAngleTolerance -> { diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index 00a8c74b..4c5ad1f6 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -4,9 +4,7 @@ package frc.robot.config; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.mechanisms.swerve.utility.PhoenixPIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; @@ -92,9 +90,6 @@ public record IMUConfig( public record LightsConfig(int deviceID) {} public record SwerveConfig( - CurrentLimitsConfigs steerMotorCurrentLimits, - CurrentLimitsConfigs driveMotorCurrentLimits, - TorqueCurrentConfigs driveMotorTorqueCurrentLimits, PhoenixPIDController snapController, boolean invertRotation, boolean invertX, diff --git a/src/main/java/frc/robot/generated/CompBotTunerConstants.java b/src/main/java/frc/robot/generated/CompBotTunerConstants.java index 0fd80013..b4a401f8 100644 --- a/src/main/java/frc/robot/generated/CompBotTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompBotTunerConstants.java @@ -4,7 +4,10 @@ package frc.robot.generated; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; @@ -69,6 +72,26 @@ public class CompBotTunerConstants { private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorInitialConfigs( + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(120) + .withStatorCurrentLimit(70) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimitEnable(true)) + .withTorqueCurrent( + new TorqueCurrentConfigs() + .withPeakForwardTorqueCurrent(80) + .withPeakReverseTorqueCurrent(-80))) + .withSteerMotorInitialConfigs( + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(20) + .withStatorCurrentLimit(70) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimitEnable(true))) .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) .withWheelRadius(kWheelRadiusInches) diff --git a/src/main/java/frc/robot/generated/PracticeBotTunerConstants.java b/src/main/java/frc/robot/generated/PracticeBotTunerConstants.java index 3c6f53d3..7e1d9a88 100644 --- a/src/main/java/frc/robot/generated/PracticeBotTunerConstants.java +++ b/src/main/java/frc/robot/generated/PracticeBotTunerConstants.java @@ -4,7 +4,10 @@ package frc.robot.generated; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; @@ -69,6 +72,26 @@ public class PracticeBotTunerConstants { private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorInitialConfigs( + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(150) + .withStatorCurrentLimit(70) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimitEnable(true)) + .withTorqueCurrent( + new TorqueCurrentConfigs() + .withPeakForwardTorqueCurrent(80) + .withPeakReverseTorqueCurrent(-80))) + .withSteerMotorInitialConfigs( + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(20) + .withStatorCurrentLimit(70) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimitEnable(true))) .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) .withWheelRadius(kWheelRadiusInches) diff --git a/src/main/java/frc/robot/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/swerve/SwerveSubsystem.java index 3a48414e..658ee82b 100644 --- a/src/main/java/frc/robot/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/swerve/SwerveSubsystem.java @@ -67,25 +67,6 @@ public class SwerveSubsystem extends LifecycleSubsystem { private final double AccelerationLimit = 3.2; private Translation2d previousVelocity = new Translation2d(); - /** - * Helper for applying current limits to swerve modules, since the CTR swerve builder API doesn't - * support it. - */ - private static void applyCurrentLimits(SwerveModule module) { - module - .getDriveMotor() - .getConfigurator() - .apply(RobotConfig.get().swerve().driveMotorCurrentLimits()); - module - .getSteerMotor() - .getConfigurator() - .apply(RobotConfig.get().swerve().steerMotorCurrentLimits()); - module - .getDriveMotor() - .getConfigurator() - .apply(RobotConfig.get().swerve().driveMotorTorqueCurrentLimits()); - } - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); private boolean snapToAngle = false; @@ -131,11 +112,6 @@ public SwerveSubsystem(CommandXboxController driveController) { driveToAngle.HeadingController.enableContinuousInput(-Math.PI, Math.PI); driveToAngle.HeadingController.setTolerance(0.02); - applyCurrentLimits(frontLeft); - applyCurrentLimits(frontRight); - applyCurrentLimits(backLeft); - applyCurrentLimits(backRight); - omegaPid.enableContinuousInput(-Math.PI, Math.PI); }