From 12c4ccb8b4ed7b1af4e334c0ab0f8ae70ca6dc23 Mon Sep 17 00:00:00 2001 From: Ashray09 Date: Sat, 5 Sep 2026 18:24:25 -0700 Subject: [PATCH] Add per-subsystem power monitor (motor current by CAN ID) Reads per-subsystem current from the TalonFX motors by CAN ID (no PDH channel mapping). New self-contained frc.robot.power package + RobotContainer wiring. Publishes to NetworkTables, a Power (motors) Shuffleboard tab, and DataLog; also opens the REV PDH for full-system total + bus voltage. Co-Authored-By: Claude Opus 4.8 --- src/main/java/frc/robot/RobotContainer.java | 46 +++ .../frc/robot/power/BatteryEstimator.java | 146 +++++++++ .../frc/robot/power/BreakerThermalModel.java | 64 ++++ .../frc/robot/power/FinanceDepartment.java | 89 ++++++ .../frc/robot/power/MotorCurrentMonitor.java | 297 ++++++++++++++++++ .../java/frc/robot/power/PowerConstants.java | 23 ++ 6 files changed, 665 insertions(+) create mode 100644 src/main/java/frc/robot/power/BatteryEstimator.java create mode 100644 src/main/java/frc/robot/power/BreakerThermalModel.java create mode 100644 src/main/java/frc/robot/power/FinanceDepartment.java create mode 100644 src/main/java/frc/robot/power/MotorCurrentMonitor.java create mode 100644 src/main/java/frc/robot/power/PowerConstants.java 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: + *

+ * 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; +}