Add power monitor: per-subsystem current by CAN ID (no PDH mapping)

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()));

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).
+ *
+ * <p>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:
+ *
+ * <pre>
+ *   V_terminal = OCV(SOC) - I*R0 - V_rc      (V_rc is the polarization branch)
+ *   dV_rc/dt   = I/C1 - V_rc/(R1*C1)
+ * </pre>
+ *
+ * <p>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.
+ *
+ * <p>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));
+  }
+}

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).
+ *
+ * <p>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}:
+ *
+ * <pre>
+ *   dTheta/dt = ((I/I_rated)^2 - theta) / tau
+ * </pre>
+ *
+ * <p>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 &gt; 1) and
+ * exponentially decays during cooldowns (theta relaxes toward (I/I_rated)^2 &lt; 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;
+  }
+}

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.
+ *
+ * <p>Each loop it forward-projects both models over a time budget and binary-searches the largest
+ * total bus current that keeps:
+ * <ul>
+ *   <li>projected battery terminal voltage above brownout (+ a safety margin), and
+ *   <li>projected breaker thermal state below trip (+ a margin).
+ * </ul>
+ * 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;
+  }
+}