diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index fa93d7ff..ef13d68c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -45,6 +45,14 @@
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import frc.robot.subsystems.vision.LimelightVision;
import frc.robot.subsystems.vision.VisionSubsystem;
+import com.ctre.phoenix6.hardware.TalonFX;
+import java.util.function.DoubleSupplier;
+import frc.robot.Constants.ShooterConstants;
+import frc.robot.Constants.FeederConstants;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.Constants.HoodConstants;
+import frc.robot.Constants.TurretConstants;
+import frc.robot.power.MotorCurrentMonitor;
public class RobotContainer {
@@ -64,6 +72,9 @@ public class RobotContainer {
private final Intake intake;
private final LED led;
+ // Power monitoring: per-subsystem current sourced from the TalonFX motors by CAN ID.
+ private final MotorCurrentMonitor powerMonitor = new MotorCurrentMonitor();
+
private final ShotCalculator shotCalculator = new ShotCalculator();
private final PassCalculator passCalculator = new PassCalculator();
private Command shotCommand;
@@ -89,6 +100,7 @@ public RobotContainer() {
feeder = new Feeder(new FeederIOTalonFX());
intake = new Intake(new IntakeIOTalonFX());
led = new LED(new LEDIOAddressable(3, 65), hood, shooter, turret, shotCalculator);
+ configurePowerMonitor();
} else {
hood = new Hood(new HoodIO() {});
shooter = new Shooter(new ShooterIO() {});
@@ -159,6 +171,40 @@ private ChassisSpeeds clampSpeedsForShooting(ChassisSpeeds speeds) {
return new ChassisSpeeds(vx, vy, speeds.omegaRadiansPerSecond);
}
+ /** Read-only supply-current source for a TalonFX by CAN ID on the given CAN bus. */
+ private static DoubleSupplier talon(int canId, String canbus) {
+ TalonFX fx = new TalonFX(canId, canbus);
+ var sig = fx.getSupplyCurrent();
+ sig.setUpdateFrequency(50);
+ return () -> sig.refresh().getValueAsDouble();
+ }
+
+ /** Registers every subsystem's motor current with the power monitor (by CAN ID). */
+ private void configurePowerMonitor() {
+ final String CANIVORE = "Canivore";
+ final String RIO = "rio";
+ powerMonitor.driveGroup("Swerve Drive", 160.0)
+ .addMotor(talon(1, CANIVORE)).addMotor(talon(4, CANIVORE))
+ .addMotor(talon(7, CANIVORE)).addMotor(talon(10, CANIVORE));
+ powerMonitor.group("Swerve Steer", 100.0)
+ .addMotor(talon(3, CANIVORE)).addMotor(talon(6, CANIVORE))
+ .addMotor(talon(9, CANIVORE)).addMotor(talon(12, CANIVORE));
+ powerMonitor.group("Shooter", 100.0)
+ .addMotor(talon(ShooterConstants.LEFT_FLYWHEEL_ID, RIO))
+ .addMotor(talon(ShooterConstants.RIGHT_FLYWHEEL_ID, RIO));
+ powerMonitor.group("Feeder", 120.0)
+ .addMotor(talon(FeederConstants.PAN_MOTOR_ID, RIO))
+ .addMotor(talon(FeederConstants.FLOOR_ID, RIO))
+ .addMotor(talon(FeederConstants.PUSHER_MOTOR_ID, RIO));
+ powerMonitor.group("Intake", 60.0)
+ .addMotor(talon(IntakeConstants.LEFT_MOTOR_ID, RIO))
+ .addMotor(talon(IntakeConstants.RIGHT_MOTOR_ID, RIO));
+ powerMonitor.group("Hood", 40.0)
+ .addMotor(talon(HoodConstants.HOOD_MOTOR_ID, RIO));
+ powerMonitor.group("Turret", 30.0)
+ .addMotor(talon(TurretConstants.MOTOR_ID, RIO));
+ }
+
private void configureBindings() {
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(
() -> clampSpeedsForShooting(driveAngularVelocity.get()));
diff --git a/src/main/java/frc/robot/power/BatteryEstimator.java b/src/main/java/frc/robot/power/BatteryEstimator.java
new file mode 100644
index 00000000..a980a58b
--- /dev/null
+++ b/src/main/java/frc/robot/power/BatteryEstimator.java
@@ -0,0 +1,146 @@
+package frc.robot.power;
+
+/**
+ * Battery state estimator (inspired by Team 6328 "Mechanical Advantage" energy tracking).
+ *
+ * Estimates State of Charge (SOC) by coulomb counting with a dynamic Peukert correction, and
+ * models transient voltage sag with a single-RC Thevenin equivalent circuit:
+ *
+ *
+ * V_terminal = OCV(SOC) - I*R0 - V_rc (V_rc is the polarization branch)
+ * dV_rc/dt = I/C1 - V_rc/(R1*C1)
+ *
+ *
+ * The measured terminal voltage is compared to the model's prediction and the SOC estimate is
+ * nudged toward what the measurement implies, using a small noise-attenuated (Kalman-style)
+ * scalar gain on the voltage innovation. OCV and internal resistance are empirical functions of
+ * SOC.
+ *
+ *
All resistances in ohms, capacity in amp-hours, time in seconds, current in amps (positive =
+ * discharge).
+ */
+public class BatteryEstimator {
+
+ // ---- Battery parameters (typical 18 Ah FRC SLA pack) ----
+ private final double nominalCapacityAh;
+ private final double r0Base = 0.012; // ohmic series resistance at full charge, fresh pack
+ private final double r1 = 0.010; // polarization resistance
+ private final double c1 = 500.0; // polarization capacitance -> tau1 = R1*C1 = 5 s
+ private final double peukertK = 1.08; // >1: high current depletes charge faster
+ private final double iRef = 20.0; // reference current for the Peukert correction
+
+ /** Kalman-style innovation gain (per volt of error, applied to SOC). Small = trust the model. */
+ private final double socGain = 0.02;
+
+ private double ageFactor; // 0 = fresh, 1 = old (scales internal resistance up)
+
+ // ---- Estimator state ----
+ private double soc; // 0..1
+ private double vRc; // polarization branch voltage
+ private double predictedVoltage;
+ private double lastCurrent;
+
+ public BatteryEstimator(double nominalCapacityAh, double initialSoc, double ageFactor) {
+ this.nominalCapacityAh = nominalCapacityAh;
+ this.soc = clamp01(initialSoc);
+ this.ageFactor = ageFactor;
+ this.predictedVoltage = openCircuitVoltage(soc);
+ }
+
+ public void setAgeFactor(double ageFactor) {
+ this.ageFactor = ageFactor;
+ }
+
+ /**
+ * Empirical open-circuit voltage as a function of SOC. Shaped like a lead-acid discharge curve:
+ * a shelf in the mid range, steeper near the ends. ~12.9 V full, ~11.6 V empty.
+ */
+ public double openCircuitVoltage(double s) {
+ s = clamp01(s);
+ // Smooth curve: base + linear + gentle S from the tanh term.
+ return 11.6 + 1.0 * s + 0.30 * Math.tanh(6.0 * (s - 0.5)) + 0.15;
+ }
+
+ /** Series (ohmic) resistance rises as the pack empties and with age. */
+ public double seriesResistance() {
+ double socPenalty = 1.0 + 0.6 * (1.0 - soc); // up to +60% when empty
+ double agePenalty = 1.0 + 1.2 * ageFactor; // up to +120% for an old pack
+ return r0Base * socPenalty * agePenalty;
+ }
+
+ /**
+ * Advance the estimator one step.
+ *
+ * @param measuredVoltage terminal voltage read from the PDH (V)
+ * @param current total bus current (A, positive = discharge)
+ * @param dt timestep (s)
+ */
+ public void update(double measuredVoltage, double current, double dt) {
+ this.lastCurrent = current;
+
+ // 1) Coulomb counting with dynamic Peukert correction.
+ double iEff = current * Math.pow(Math.max(current, 0.1) / iRef, peukertK - 1.0);
+ soc -= (iEff * dt) / (3600.0 * nominalCapacityAh);
+ soc = clamp01(soc);
+
+ // 2) Propagate the RC polarization branch (exact discrete solution).
+ double tau1 = r1 * c1;
+ double decay = Math.exp(-dt / tau1);
+ vRc = vRc * decay + current * r1 * (1.0 - decay);
+
+ // 3) Model-predicted terminal voltage.
+ predictedVoltage = openCircuitVoltage(soc) - current * seriesResistance() - vRc;
+
+ // 4) Kalman-style SOC correction from the voltage innovation, scaled by dOCV/dSOC so the
+ // volt error maps to a SOC error. Gain kept small to attenuate sensor noise.
+ double innovation = measuredVoltage - predictedVoltage;
+ double dOcvDsoc = ocvSlope(soc);
+ if (dOcvDsoc > 1e-3) {
+ soc = clamp01(soc + socGain * innovation / dOcvDsoc);
+ }
+ }
+
+ /** Numerical dOCV/dSOC. */
+ private double ocvSlope(double s) {
+ double h = 0.01;
+ return (openCircuitVoltage(s + h) - openCircuitVoltage(s - h)) / (2 * h);
+ }
+
+ /**
+ * Forward-project the minimum terminal voltage if the bus were held at {@code totalCurrent} for
+ * {@code horizonSec}. Used by the current-budget projector to find the largest draw that keeps
+ * voltage above brownout. Does not mutate estimator state.
+ */
+ public double projectMinVoltage(double totalCurrent, double horizonSec, double dt) {
+ double s = soc;
+ double v = vRc;
+ double tau1 = r1 * c1;
+ double decay = Math.exp(-dt / tau1);
+ double minV = Double.MAX_VALUE;
+ for (double t = 0; t < horizonSec; t += dt) {
+ double iEff = totalCurrent * Math.pow(Math.max(totalCurrent, 0.1) / iRef, peukertK - 1.0);
+ s = clamp01(s - (iEff * dt) / (3600.0 * nominalCapacityAh));
+ v = v * decay + totalCurrent * r1 * (1.0 - decay);
+ double r0 = r0Base * (1.0 + 0.6 * (1.0 - s)) * (1.0 + 1.2 * ageFactor);
+ double vTerm = openCircuitVoltage(s) - totalCurrent * r0 - v;
+ if (vTerm < minV) minV = vTerm;
+ }
+ return minV;
+ }
+
+ public double soc() {
+ return soc;
+ }
+
+ public double predictedVoltage() {
+ return predictedVoltage;
+ }
+
+ public double lastCurrent() {
+ return lastCurrent;
+ }
+
+ private static double clamp01(double x) {
+ return Math.max(0.0, Math.min(1.0, x));
+ }
+}
diff --git a/src/main/java/frc/robot/power/BreakerThermalModel.java b/src/main/java/frc/robot/power/BreakerThermalModel.java
new file mode 100644
index 00000000..4fb30e8f
--- /dev/null
+++ b/src/main/java/frc/robot/power/BreakerThermalModel.java
@@ -0,0 +1,64 @@
+package frc.robot.power;
+
+/**
+ * Thermal model of the 120 A main breaker (inspired by Team 6328's approach).
+ *
+ *
A thermal-magnetic breaker trips on accumulated heat, not instantaneous current. We track a
+ * normalized thermal state {@code theta} (0 = cold, 1 = trip) with a first-order model whose
+ * steady state is {@code (I/I_rated)^2}:
+ *
+ *
+ * dTheta/dt = ((I/I_rated)^2 - theta) / tau
+ *
+ *
+ * This is the physical realization of the "Miner's rule" damage accumulation described in the
+ * binder: heat accumulates during high-current draws (theta rises toward (I/I_rated)^2 > 1) and
+ * exponentially decays during cooldowns (theta relaxes toward (I/I_rated)^2 < 1). The breaker
+ * tolerates brief overcurrent because theta lags the current.
+ */
+public class BreakerThermalModel {
+
+ private final double ratedAmps;
+ private final double tauSeconds; // thermal time constant
+
+ private double theta; // 0..1, trip at 1
+
+ public BreakerThermalModel(double ratedAmps, double tauSeconds) {
+ this.ratedAmps = ratedAmps;
+ this.tauSeconds = tauSeconds;
+ this.theta = 0.0;
+ }
+
+ /** Advance the thermal state one step at the given total current. */
+ public void update(double totalCurrent, double dt) {
+ double drive = Math.pow(totalCurrent / ratedAmps, 2.0);
+ theta += (drive - theta) * (dt / tauSeconds);
+ theta = Math.max(0.0, theta);
+ }
+
+ /** Normalized thermal state 0..1 (fraction of the way to a trip). */
+ public double thermalState() {
+ return theta;
+ }
+
+ public boolean isTripped() {
+ return theta >= 1.0;
+ }
+
+ /**
+ * Forward-project the peak thermal state if the bus were held at {@code totalCurrent} for
+ * {@code horizonSec}. Does not mutate state.
+ */
+ public double projectMaxTheta(double totalCurrent, double horizonSec, double dt) {
+ double drive = Math.pow(totalCurrent / ratedAmps, 2.0);
+ double th = theta;
+ for (double t = 0; t < horizonSec; t += dt) {
+ th += (drive - th) * (dt / tauSeconds);
+ }
+ return th;
+ }
+
+ public double ratedAmps() {
+ return ratedAmps;
+ }
+}
diff --git a/src/main/java/frc/robot/power/FinanceDepartment.java b/src/main/java/frc/robot/power/FinanceDepartment.java
new file mode 100644
index 00000000..cea68bcb
--- /dev/null
+++ b/src/main/java/frc/robot/power/FinanceDepartment.java
@@ -0,0 +1,89 @@
+package frc.robot.power;
+
+// self-contained thresholds
+
+/**
+ * The "finance department" (Team 6328 term): allocates current to the drivetrain based on the
+ * battery estimator and breaker thermal model, so the robot can safely use the full potential of
+ * the battery in every match instead of a fixed conservative limit.
+ *
+ *
Each loop it forward-projects both models over a time budget and binary-searches the largest
+ * total bus current that keeps:
+ *
+ * - projected battery terminal voltage above brownout (+ a safety margin), and
+ *
- projected breaker thermal state below trip (+ a margin).
+ *
+ * The reserved draw of the non-drive subsystems is subtracted from that ceiling, and whatever is
+ * left is handed to the drivetrain as its dynamic current limit.
+ */
+public class FinanceDepartment {
+
+ private final BatteryEstimator battery;
+ private final BreakerThermalModel breaker;
+
+ // Projection horizons (s) and integration step for the forward simulation.
+ private final double batteryHorizon = 1.5;
+ private final double breakerHorizon = 3.0;
+ private final double projectionDt = 0.02;
+
+ // Safety margins.
+ private final double brownoutMargin = 0.3; // keep projected V >= brownout + 0.3 V
+ private final double thetaMargin = 0.85; // keep projected theta <= 0.85
+
+ // Search bounds for total permissible current.
+ private final double searchMin = 0.0;
+ private final double searchMax = 400.0;
+
+ private double permissibleTotal;
+ private double driveAllocation;
+
+ public FinanceDepartment(BatteryEstimator battery, BreakerThermalModel breaker) {
+ this.battery = battery;
+ this.breaker = breaker;
+ }
+
+ /**
+ * Recompute the budget.
+ *
+ * @param reservedNonDriveCurrent current the non-drive subsystems are drawing / expected to draw
+ * @return the drivetrain's allocated current limit (A)
+ */
+ public double allocate(double reservedNonDriveCurrent) {
+ permissibleTotal = maxPermissibleTotalCurrent();
+ driveAllocation = Math.max(0.0, permissibleTotal - reservedNonDriveCurrent);
+ return driveAllocation;
+ }
+
+ /** Binary-search the largest total current that satisfies both projected constraints. */
+ private double maxPermissibleTotalCurrent() {
+ double lo = searchMin;
+ double hi = searchMax;
+ // If even a tiny current already violates (empty/hot), the loop returns lo ~ 0.
+ for (int i = 0; i < 24; i++) {
+ double mid = 0.5 * (lo + hi);
+ if (feasible(mid)) {
+ lo = mid;
+ } else {
+ hi = mid;
+ }
+ }
+ return lo;
+ }
+
+ private boolean feasible(double totalCurrent) {
+ double minV = battery.projectMinVoltage(totalCurrent, batteryHorizon, projectionDt);
+ if (minV < PowerConstants.BROWNOUT_VOLTAGE + brownoutMargin) return false;
+ double maxTheta = breaker.projectMaxTheta(totalCurrent, breakerHorizon, projectionDt);
+ return maxTheta <= thetaMargin;
+ }
+
+ /** Last computed maximum permissible total bus current (A). */
+ public double permissibleTotalCurrent() {
+ return permissibleTotal;
+ }
+
+ /** Last computed drivetrain allocation (A). */
+ public double driveAllocation() {
+ return driveAllocation;
+ }
+}
diff --git a/src/main/java/frc/robot/power/MotorCurrentMonitor.java b/src/main/java/frc/robot/power/MotorCurrentMonitor.java
new file mode 100644
index 00000000..cb1093c9
--- /dev/null
+++ b/src/main/java/frc/robot/power/MotorCurrentMonitor.java
@@ -0,0 +1,297 @@
+package frc.robot.power;
+
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.function.DoubleSupplier;
+
+/**
+ * Per-subsystem power monitor that sources current from the motor controllers (by CAN ID),
+ * the way Team 6328 / AdvantageKit do it — NOT from PDH output channels. Each subsystem is a named
+ * group of motor current suppliers; you provide the per-motor current from your existing motor
+ * objects (Kraken/TalonFX {@code getSupplyCurrent()}, SPARK MAX {@code getOutputCurrent()}), so no
+ * PDH port mapping / wire tracing is needed.
+ *
+ * This class is deliberately vendor-agnostic: it only takes {@link DoubleSupplier}s of
+ * amps, so it compiles against plain WPILib and works with any motor library. You wire the actual
+ * Phoenix 6 / REVLib calls in your {@code RobotContainer} (see the integration guide).
+ *
+ *
Bus voltage and brownout come from {@link RobotController} (no PDH required). It reuses the
+ * {@link BatteryEstimator}, {@link BreakerThermalModel}, and {@link FinanceDepartment} to predict
+ * sag, track breaker heat, and compute the dynamic drive-current allocation.
+ *
+ *
{@code
+ * // in RobotContainer, after your motors exist:
+ * var monitor = new MotorCurrentMonitor();
+ * monitor.driveGroup("Swerve Drive", BreakerRatings.DRIVE)
+ * .addMotor(() -> flDrive.getSupplyCurrent().getValueAsDouble())
+ * .addMotor(() -> frDrive.getSupplyCurrent().getValueAsDouble()) ...;
+ * monitor.group("Elevator", BreakerRatings.INDEXER)
+ * .addMotor(() -> elevatorLeft.getOutputCurrent())
+ * .addMotor(() -> elevatorRight.getOutputCurrent());
+ * }
+ */
+public class MotorCurrentMonitor extends SubsystemBase {
+
+ private static final double WARNING_THROTTLE_SECONDS = 1.0;
+ private static final double NEAR_BREAKER_FRACTION = 0.90;
+
+ /** A named subsystem = one or more motor current suppliers, summed each loop. */
+ public static final class Group {
+ private final String name;
+ private final boolean isDrive; // the drivetrain: protected, never auto-shed
+ private final double breakerAmps;
+ private final List motors = new ArrayList<>();
+ private double current; // last summed current (A)
+
+ private Group(String name, boolean isDrive, double breakerAmps) {
+ this.name = name;
+ this.isDrive = isDrive;
+ this.breakerAmps = breakerAmps;
+ }
+
+ /** Add one motor's current source (amps). Chainable. */
+ public Group addMotor(DoubleSupplier currentAmps) {
+ motors.add(currentAmps);
+ return this;
+ }
+
+ private double sum() {
+ double s = 0;
+ for (DoubleSupplier m : motors) {
+ s += Math.max(0.0, m.getAsDouble());
+ }
+ current = s;
+ return s;
+ }
+
+ public String name() {
+ return name;
+ }
+
+ public double current() {
+ return current;
+ }
+
+ public double breakerAmps() {
+ return breakerAmps;
+ }
+ }
+
+ private final List groups = new ArrayList<>();
+
+ // Estimation / allocation (reused, unchanged).
+ private final BatteryEstimator battery = new BatteryEstimator(18.0, 1.0, 0.3);
+ private final BreakerThermalModel breaker =
+ new BreakerThermalModel(PowerConstants.MAIN_BREAKER_AMPS, 40.0);
+ private final FinanceDepartment finance = new FinanceDepartment(battery, breaker);
+ private volatile double driveCurrentAllocation = PowerConstants.MAIN_BREAKER_AMPS;
+ private boolean financeEnabled = true;
+
+ // Optional REV PDH for full-system truth (total current + bus voltage). Reading a PDH that
+ // isn't present just returns ~0, so we sanity-check its voltage and fall back to the roboRIO.
+ private final PowerDistribution pdh =
+ new PowerDistribution(PowerConstants.PDH_CAN_ID, PowerDistribution.ModuleType.kRev);
+
+ // NetworkTables + DataLog.
+ private final NetworkTable table = NetworkTableInstance.getDefault().getTable("PowerMonitor");
+ private final DoublePublisher totalCurrentPub = table.getDoubleTopic("MotorTotalCurrent").publish();
+ private final DoublePublisher pdhTotalPub = table.getDoubleTopic("PdhTotalCurrent").publish();
+ private final DoublePublisher busVoltagePub = table.getDoubleTopic("BusVoltage").publish();
+ private final DoublePublisher socPub = table.getDoubleTopic("estimator/SOC").publish();
+ private final DoublePublisher thermalPub = table.getDoubleTopic("estimator/BreakerThermal").publish();
+ private final DoublePublisher permissiblePub = table.getDoubleTopic("finance/PermissibleTotalA").publish();
+ private final DoublePublisher driveAllocPub = table.getDoubleTopic("finance/DriveAllocationA").publish();
+ private final StringPublisher statusPub = table.getStringTopic("Status").publish();
+ private final List groupPub = new ArrayList<>();
+ private final List groupLog = new ArrayList<>();
+
+ private final DataLog log = DataLogManager.getLog();
+ private final DoubleLogEntry logTotal = new DoubleLogEntry(log, "/power/motorTotalCurrent");
+ private final DoubleLogEntry logVoltage = new DoubleLogEntry(log, "/power/busVoltage");
+ private final DoubleLogEntry logSoc = new DoubleLogEntry(log, "/power/estimator/soc");
+
+ private final ShuffleboardTab tab = Shuffleboard.getTab("Power (motors)");
+
+ // Brownout + alert throttling.
+ private int brownoutCount = 0;
+ private boolean wasBrownedOut = false;
+ private double minVoltage = PowerConstants.NOMINAL_VOLTAGE;
+ private double lastLowVoltageWarn = -WARNING_THROTTLE_SECONDS;
+ private double lastHighCurrentWarn = -WARNING_THROTTLE_SECONDS;
+ private double lastBreakerWarn = -WARNING_THROTTLE_SECONDS;
+
+ public MotorCurrentMonitor() {
+ DataLogManager.start();
+ DriverStation.startDataLog(log);
+ }
+
+ /** Register the drivetrain group (protected from load-shedding, gets the finance allocation). */
+ public Group driveGroup(String name, double breakerAmps) {
+ return addGroup(name, true, breakerAmps);
+ }
+
+ /** Register a non-drive subsystem group. */
+ public Group group(String name, double breakerAmps) {
+ return addGroup(name, false, breakerAmps);
+ }
+
+ private Group addGroup(String name, boolean isDrive, double breakerAmps) {
+ Group g = new Group(name, isDrive, breakerAmps);
+ groups.add(g);
+ groupPub.add(table.getDoubleTopic("motor/" + name).publish());
+ groupLog.add(new DoubleLogEntry(log, "/power/motor/" + name));
+ tab.addDouble(name + " (A)", g::current)
+ .withWidget(BuiltInWidgets.kNumberBar)
+ .withProperties(Map.of("Min", 0, "Max", breakerAmps));
+ return g;
+ }
+
+ @Override
+ public void periodic() {
+ final double now = Timer.getFPGATimestamp();
+ final double dt = PowerConstants.LOOP_PERIOD_SECONDS;
+
+ // 1) Sum each subsystem's motor currents; track the protected drive group separately.
+ double motorTotal = 0;
+ double driveCurrent = 0;
+ for (Group g : groups) {
+ double c = g.sum();
+ motorTotal += c;
+ if (g.isDrive) driveCurrent += c;
+ }
+
+ // 2) Prefer the PDH for full-system total + bus voltage (captures non-motor loads too);
+ // fall back to the roboRIO if no PDH is present (its reads sanity-check to ~0V).
+ final double pdhVoltage = pdh.getVoltage();
+ final boolean pdhOk = pdhVoltage > 4.0;
+ final double pdhTotal = pdh.getTotalCurrent();
+ final double voltage = pdhOk ? pdhVoltage : RobotController.getBatteryVoltage();
+ final double total = pdhOk ? pdhTotal : motorTotal; // system total for estimation
+ final double reservedNonDrive = Math.max(0.0, total - driveCurrent);
+ updateBrownout(voltage);
+
+ // 3) Estimate + allocate on the system total.
+ battery.update(voltage, total, dt);
+ breaker.update(total, dt);
+ if (financeEnabled) {
+ driveCurrentAllocation = finance.allocate(reservedNonDrive);
+ }
+
+ publishAndLog(motorTotal, pdhTotal, voltage);
+ runAlerts(now, voltage, total);
+ statusPub.set(statusFor(voltage, total));
+ }
+
+ private void updateBrownout(double voltage) {
+ if (voltage < minVoltage) minVoltage = voltage;
+ boolean browned = RobotController.isBrownedOut();
+ if (browned && !wasBrownedOut) {
+ brownoutCount++;
+ DriverStation.reportError("BROWNOUT #" + brownoutCount + " (bus " + round(voltage) + "V)", false);
+ table.getDoubleTopic("BrownoutCount").publish().set(brownoutCount);
+ }
+ wasBrownedOut = browned;
+ }
+
+ private void publishAndLog(double motorTotal, double pdhTotal, double voltage) {
+ totalCurrentPub.set(motorTotal);
+ pdhTotalPub.set(pdhTotal);
+ busVoltagePub.set(voltage);
+ socPub.set(battery.soc());
+ thermalPub.set(breaker.thermalState());
+ permissiblePub.set(finance.permissibleTotalCurrent());
+ driveAllocPub.set(driveCurrentAllocation);
+
+ logTotal.append(motorTotal);
+ logVoltage.append(voltage);
+ logSoc.append(battery.soc());
+ for (int i = 0; i < groups.size(); i++) {
+ double c = groups.get(i).current();
+ groupPub.get(i).set(c);
+ groupLog.get(i).append(c);
+ }
+ }
+
+ private void runAlerts(double now, double voltage, double total) {
+ if (voltage < PowerConstants.LOW_VOLTAGE_WARNING && now - lastLowVoltageWarn > WARNING_THROTTLE_SECONDS) {
+ DriverStation.reportWarning("Low bus voltage " + round(voltage) + "V - brownout risk", false);
+ lastLowVoltageWarn = now;
+ }
+ if (total > PowerConstants.TOTAL_CURRENT_BUDGET_AMPS && now - lastHighCurrentWarn > WARNING_THROTTLE_SECONDS) {
+ DriverStation.reportWarning("Motor current " + round(total) + "A - approaching budget", false);
+ lastHighCurrentWarn = now;
+ }
+ for (Group g : groups) {
+ if (g.current() > g.breakerAmps() * NEAR_BREAKER_FRACTION
+ && now - lastBreakerWarn > WARNING_THROTTLE_SECONDS) {
+ DriverStation.reportWarning(
+ g.name() + " " + round(g.current()) + "A near " + round(g.breakerAmps()) + "A breaker", false);
+ lastBreakerWarn = now;
+ }
+ }
+ }
+
+ private String statusFor(double voltage, double total) {
+ if (voltage < PowerConstants.LOW_VOLTAGE_WARNING || total > PowerConstants.TOTAL_CURRENT_BUDGET_AMPS) return "RED";
+ if (total > PowerConstants.TOTAL_CURRENT_CAUTION_AMPS) return "YELLOW";
+ return "GREEN";
+ }
+
+ private static double round(double v) {
+ return Math.round(v * 10.0) / 10.0;
+ }
+
+ // ---- Accessors for your mechanisms / commands ----
+
+ /** Dynamic current limit (A) the finance dept. allocates to the drivetrain this loop. */
+ public double driveCurrentAllocation() {
+ return driveCurrentAllocation;
+ }
+
+ public double stateOfCharge() {
+ return battery.soc();
+ }
+
+ public double breakerThermalState() {
+ return breaker.thermalState();
+ }
+
+ public double permissibleTotalCurrent() {
+ return finance.permissibleTotalCurrent();
+ }
+
+ public int brownoutCount() {
+ return brownoutCount;
+ }
+
+ public double minBusVoltage() {
+ return minVoltage;
+ }
+
+ public void setFinanceEnabled(boolean enabled) {
+ financeEnabled = enabled;
+ if (!enabled) driveCurrentAllocation = PowerConstants.MAIN_BREAKER_AMPS;
+ }
+
+ public void setBatteryAge(double ageFactor) {
+ battery.setAgeFactor(ageFactor);
+ }
+}
diff --git a/src/main/java/frc/robot/power/PowerConstants.java b/src/main/java/frc/robot/power/PowerConstants.java
new file mode 100644
index 00000000..1405491f
--- /dev/null
+++ b/src/main/java/frc/robot/power/PowerConstants.java
@@ -0,0 +1,23 @@
+package frc.robot.power;
+
+/**
+ * Self-contained power thresholds for the portable {@code power} package, so it drops into any
+ * robot project without depending on that project's own {@code Constants}. Tune to taste.
+ */
+public final class PowerConstants {
+ private PowerConstants() {}
+
+ /** REV PDH CAN ID (REV default is 1). */
+ public static final int PDH_CAN_ID = 1;
+
+ /** Robot loop period (s). */
+ public static final double LOOP_PERIOD_SECONDS = 0.020;
+
+ // ---- Budget / brownout thresholds ----
+ public static final double MAIN_BREAKER_AMPS = 120.0;
+ public static final double TOTAL_CURRENT_BUDGET_AMPS = 90.0; // practical sustained ceiling
+ public static final double TOTAL_CURRENT_CAUTION_AMPS = 70.0; // green/yellow boundary
+ public static final double LOW_VOLTAGE_WARNING = 7.0; // brownout-risk warning
+ public static final double BROWNOUT_VOLTAGE = 6.8; // RoboRIO brownout threshold
+ public static final double NOMINAL_VOLTAGE = 12.0;
+}