001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.simulation;
006
007/** A utility class to simulate the robot battery. */
008public final class BatterySim {
009  private BatterySim() {
010    // Utility class
011  }
012
013  /**
014   * Calculate the loaded battery voltage. Use this with {@link RoboRioSim#setVInVoltage(double)} to
015   * set the simulated battery voltage, which can then be retrieved with the {@link
016   * edu.wpi.first.wpilibj.RobotController#getBatteryVoltage()} method.
017   *
018   * @param nominalVoltage The nominal battery voltage. Usually 12v.
019   * @param resistanceOhms The forward resistance of the battery. Most batteries are at or below 20
020   *     milliohms.
021   * @param currents The currents drawn from the battery.
022   * @return The battery's voltage under load.
023   */
024  public static double calculateLoadedBatteryVoltage(
025      double nominalVoltage, double resistanceOhms, double... currents) {
026    double retval = nominalVoltage;
027    for (var current : currents) {
028      retval -= current * resistanceOhms;
029    }
030    return retval;
031  }
032
033  /**
034   * Calculate the loaded battery voltage. Use this with {@link RoboRioSim#setVInVoltage(double)} to
035   * set the simulated battery voltage, which can then be retrieved with the {@link
036   * edu.wpi.first.wpilibj.RobotController#getBatteryVoltage()} method. This function assumes a
037   * nominal voltage of 12v and a resistance of 20 milliohms (0.020 ohms)
038   *
039   * @param currents The currents drawn from the battery.
040   * @return The battery's voltage under load.
041   */
042  public static double calculateDefaultBatteryLoadedVoltage(double... currents) {
043    return calculateLoadedBatteryVoltage(12, 0.02, currents);
044  }
045}