[wpilib] Add physics simulation support with state-space (#2615)

This includes physics simulation support for arms/elevator models, as well as differential drivetrains.

Swerve might be added at a later date.

Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibc/src/main/native/include/frc/simulation/BatterySim.h

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <numeric>
+
+#include <units/current.h>
+#include <units/impedance.h>
+#include <units/voltage.h>
+
+namespace frc::sim {
+
+class BatterySim {
+ public:
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
+   * voltage, which can then be retrieved with the {@link
+   * RobotController#getBatteryVoltage()} method.
+   *
+   * @param nominalVoltage The nominal battery voltage. Usually 12v.
+   * @param resistance     The forward resistance of the battery. Most batteries
+   *                       are at or below 20 milliohms.
+   * @param currents       The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  static units::volt_t Calculate(
+      units::volt_t nominalVoltage, units::ohm_t resistance,
+      std::initializer_list<units::ampere_t> currents) {
+    return nominalVoltage -
+           std::accumulate(currents.begin(), currents.end(), 0_A) * resistance;
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
+   * voltage, which can then be retrieved with the {@link
+   * RobotController#getBatteryVoltage()} method. This function assumes a
+   * nominal voltage of 12v and a resistance of 20 milliohms (0.020 ohms)
+   *
+   * @param currents The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  static units::volt_t Calculate(
+      std::initializer_list<units::ampere_t> currents) {
+    return Calculate(12_V, 0.02_Ohm, currents);
+  }
+};
+
+}  // namespace frc::sim