[wpilib] Clean up physics simulation class APIs (#2763)

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

@@ -18,7 +18,16 @@
 
 namespace frc::sim {
 /**
- * Represents a simulated generic linear system.
+ * This class helps simulate linear systems. To use this class, do the following
+ * in the simulationPeriodic() method.
+ *
+ * Call the SetInput() method with the inputs to your system (generally
+ * voltage). Call the Update() method to update the simulation. Set simulated
+ * sensor readings with the simulated positions in the GetOutput() method.
+ *
+ * @tparam States  The number of states of the system.
+ * @tparam Inputs  The number of inputs to the system.
+ * @tparam Outputs The number of outputs of the system.
  */
 template <int States, int Inputs, int Outputs>
 class LinearSystemSim {
@@ -27,53 +36,19 @@ class LinearSystemSim {
    * Creates a simulated generic linear system.
    *
    * @param system             The system to simulate.
-   * @param addNoise           Whether the sim should automatically add some
-   *                           measurement noise.
-   * @param measurementStdDevs The standard deviations of the measurement noise.
+   * @param measurementStdDevs The standard deviations of the measurements.
    */
   LinearSystemSim(const LinearSystem<States, Inputs, Outputs>& system,
-                  bool addNoise = false,
-                  const std::array<double, Outputs>& measurementStdDevs = {0.0})
-      : m_plant(system),
-        m_shouldAddNoise(addNoise),
-        m_measurementStdDevs(measurementStdDevs) {
+                  const std::array<double, Outputs>& measurementStdDevs =
+                      std::array<double, Outputs>{})
+      : m_plant(system), m_measurementStdDevs(measurementStdDevs) {
     m_x = Eigen::Matrix<double, States, 1>::Zero();
     m_y = Eigen::Matrix<double, Outputs, 1>::Zero();
     m_u = Eigen::Matrix<double, Inputs, 1>::Zero();
   }
 
   /**
-   * Returns whether the sim should add noise to the measurements.
-   *
-   * @return Whether the sim should add noise to the measurements.
-   */
-  bool ShouldAddNoise() const { return m_shouldAddNoise; }
-
-  /**
-   * Returns the current output of the plant.
-   *
-   * @return The current output of the plant.
-   */
-  const Eigen::Matrix<double, Outputs, 1>& Y() const { return m_y; }
-
-  /**
-   * Returns an element of the current output of the plant.
-   *
-   * @param row The row to return.
-   */
-  double Y(int i) const { return m_y(i); }
-
-  /**
-   * Sets whether the sim should add noise to measurements.
-   *
-   * @param shouldAddNoise Whether the sim should add noise to measurements.
-   */
-  void SetShouldAddNoise(bool shouldAddNoise) {
-    m_shouldAddNoise = shouldAddNoise;
-  }
-
-  /**
-   * Updates the linear system sim.
+   * Updates the simulation.
    *
    * @param dt The time between updates.
    */
@@ -85,12 +60,27 @@ class LinearSystemSim {
     // y = Cx + Du
     m_y = m_plant.CalculateY(m_x, m_u);
 
-    // Add noise if needed.
-    if (m_shouldAddNoise) {
-      m_y += frc::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
-    }
+    // Add noise. If the user did not pass a noise vector to the
+    // constructor, then this method will not do anything because
+    // the standard deviations default to zero.
+    m_y += frc::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
   }
 
+  /**
+   * Returns the current output of the plant.
+   *
+   * @return The current output of the plant.
+   */
+  const Eigen::Matrix<double, Outputs, 1>& GetOutput() const { return m_y; }
+
+  /**
+   * Returns an element of the current output of the plant.
+   *
+   * @param row The row to return.
+   * @return An element of the current output of the plant.
+   */
+  double GetOutput(int row) const { return m_y(row); }
+
   /**
    * Sets the system inputs (usually voltages).
    *
@@ -98,13 +88,31 @@ class LinearSystemSim {
    */
   void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
 
+  /*
+   * Sets the system inputs.
+   *
+   * @param row   The row in the input matrix to set.
+   * @param value The value to set the row to.
+   */
+  void SetInput(int row, double value) { m_u(row, 0) = value; }
+
   /**
    * Sets the system state.
    *
-   * @param state The state.
+   * @param state The new state.
    */
   void SetState(const Eigen::Matrix<double, States, 1>& state) { m_x = state; }
 
+  /**
+   * Returns the current drawn by this simulated system. Override this method to
+   * add a custom current calculation.
+   *
+   * @return The current drawn by this simulated mechanism.
+   */
+  virtual units::ampere_t GetCurrentDraw() const {
+    return units::ampere_t(0.0);
+  }
+
  protected:
   /**
    * Updates the state estimate of the system.
@@ -119,12 +127,7 @@ class LinearSystemSim {
     return m_plant.CalculateX(currentXhat, u, dt);
   }
 
-  virtual units::ampere_t GetCurrentDraw() const {
-    return units::ampere_t(0.0);
-  }
-
   LinearSystem<States, Inputs, Outputs> m_plant;
-  bool m_shouldAddNoise;
 
   Eigen::Matrix<double, States, 1> m_x;
   Eigen::Matrix<double, Outputs, 1> m_y;