diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 1248789..7c84953 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.core.CoreTalonFX; @@ -100,6 +101,7 @@ public SwerveSubsystem(File directory) setupCustom(); initDriveTalons(); setupFOC(); + setCurrentLimits(); } /** @@ -115,6 +117,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig setupCustom(); initDriveTalons(); setupFOC(); + setCurrentLimits(); } /** @@ -354,6 +357,21 @@ private void updateMatchTime() { SmartDashboard.putString("Match Time", matchTime); } + private void setCurrentLimits() { + for (SwerveModule swerveModule : swerveDrive.getModules()){ + TalonFX motor = (TalonFX) swerveModule.getDriveMotor().getMotor(); + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + config.StatorCurrentLimitEnable = true; + config.SupplyCurrentLimitEnable = true; + config.SupplyCurrentThreshold = 60; + config.SupplyTimeThreshold = 2.5; + config.StatorCurrentLimit = Constants.driveStatorCurrentLimit; + config.SupplyCurrentLimit = Constants.driveSupplyCurrentLimit; + motor.getConfigurator().apply(config); + motor.getConfigurator().refresh(config); + } + } + @Override public void simulationPeriodic() {