Current limiting changes for talon fx

frc-5160-the-chargers · Oct 9, 2024 · fadb0d4 · fadb0d4
1 parent d14ac3e
commit fadb0d4
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 16 deletions.
diff --git a/src/main/kotlin/frc/chargers/hardware/motorcontrol/ChargerTalonFX.kt b/src/main/kotlin/frc/chargers/hardware/motorcontrol/ChargerTalonFX.kt
@@ -4,6 +4,7 @@ package frc.chargers.hardware.motorcontrol
 import com.batterystaple.kmeasure.quantities.*
 import com.batterystaple.kmeasure.units.*
 import com.ctre.phoenix6.StatusCode
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs
 import com.ctre.phoenix6.configs.TalonFXConfiguration
 import com.ctre.phoenix6.controls.*
 import com.ctre.phoenix6.hardware.TalonFX
@@ -214,7 +215,7 @@ class ChargerTalonFX(
                     else -> nonTalonFXFollowers.add(follower)
                 }
             }
-            base.configurator.apply(config, 0.050).bind()
+            base.configurator.apply(config, 0.1).bind()
 
             val optimizeBusUtilization = optimizeUpdateRate == true
             if (optimizeBusUtilization) {
@@ -237,6 +238,27 @@ class ChargerTalonFX(
         return this
     }
 
+    fun limitSupplyCurrent(limit: Current): ChargerTalonFX = currentConfigImpl(limit)
+
+    /**
+     * Sets a limit of [allowUpTo] until [forTime] passes, then reverts to the [limit].
+     */
+    fun limitSupplyCurrent(limit: Current, allowUpTo: Current, forTime: Time): ChargerTalonFX =
+        currentConfigImpl(limit, allowUpTo, forTime)
+
+    private fun currentConfigImpl(limit: Current, allowUpTo: Current? = null, forTime: Time? = null): ChargerTalonFX {
+        val configs = CurrentLimitsConfigs()
+        base.configurator.refresh(configs, 0.05)
+        configs.apply {
+            SupplyCurrentLimitEnable = true
+            SupplyCurrentLimit = limit.inUnit(amps)
+            if (allowUpTo != null) SupplyCurrentThreshold = allowUpTo.inUnit(amps)
+            if (forTime != null) SupplyTimeThreshold = forTime.inUnit(seconds)
+        }
+        base.configurator.apply(configs, 0.1)
+        return this
+    }
+
     private val faultSignalToMsg = mapOf(
         base.fault_Hardware to "Hardware failure detected",
         base.fault_DeviceTemp to "Device temp exceeded limit",
@@ -255,9 +277,7 @@ class ChargerTalonFX(
                     // != false prevents null
                     if (faultSignal.refresh().value != false) HorseLog.logFault("$faultLogName: $faultMsg")
                 }
-                if (voltageSignal.refresh().status != StatusCode.OK) {
-                    HorseLog.logFault("$faultLogName: Device is unreachable")
-                }
+                if (voltageSignal.refresh().status != StatusCode.OK) HorseLog.logFault("$faultLogName: Device is unreachable")
             }
         }
     }

diff --git a/src/main/kotlin/frc/robot/rigatoni/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/rigatoni/subsystems/Drivetrain.kt
@@ -2,7 +2,6 @@ package frc.robot.rigatoni.subsystems
 
 import com.batterystaple.kmeasure.quantities.times
 import com.batterystaple.kmeasure.units.*
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs
 import com.pathplanner.lib.util.PIDConstants
 import edu.wpi.first.math.system.plant.DCMotor
 import edu.wpi.first.wpilibj.TimedRobot.isSimulation
@@ -62,8 +61,7 @@ private val TURN_MOTORS = listOf(
 ).map {
     it.configure(
         statorCurrentLimit = 30.amps,
-        rampRate = 48.seconds,
-        //optimizeUpdateRate = true
+        rampRate = 48.seconds
     )
 }
 private val TURN_ENCODERS = listOf(
@@ -79,16 +77,10 @@ private val DRIVE_MOTORS = listOf(
     ChargerTalonFX(DrivetrainID.BL_DRIVE),
     ChargerTalonFX(DrivetrainID.BR_DRIVE).configure(inverted = true),
 ).map {
-    it.base.configurator.apply(
-        CurrentLimitsConfigs()
-            .withSupplyCurrentLimitEnable(true)
-            .withSupplyCurrentLimit(90.0)
-    )
     it.configure(
-        statorCurrentLimit = 55.amps,
-        brakeWhenIdle = true,
-        //optimizeUpdateRate = true
-    )
+        statorCurrentLimit = 90.amps,
+        brakeWhenIdle = true
+    ).limitSupplyCurrent(60.amps)
 }
 private val TURN_MOTOR_MOI = 0.004.kilo.grams * (meters * meters)
 private val DRIVE_MOTOR_MOI = 0.025.kilo.grams * (meters * meters)