[wpilib] DCMotorSim cleanup/enhancement (#7021)

Co-authored-by: Tyler Veness <[email protected]>

Author: narmstro2020

wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp

@@ -12,17 +12,30 @@ using namespace frc;
 using namespace frc::sim;
 
 DCMotorSim::DCMotorSim(const LinearSystem<2, 1, 2>& plant,
-                       const DCMotor& gearbox, double gearing,
+                       const DCMotor& gearbox,
                        const std::array<double, 2>& measurementStdDevs)
     : LinearSystemSim<2, 1, 2>(plant, measurementStdDevs),
       m_gearbox(gearbox),
-      m_gearing(gearing) {}
-
-DCMotorSim::DCMotorSim(const DCMotor& gearbox, double gearing,
-                       units::kilogram_square_meter_t moi,
-                       const std::array<double, 2>& measurementStdDevs)
-    : DCMotorSim(LinearSystemId::DCMotorSystem(gearbox, moi, gearing), gearbox,
-                 gearing, measurementStdDevs) {}
+      // By theorem 6.10.1 of
+      // https://file.tavsys.net/control/controls-engineering-in-frc.pdf, the
+      // flywheel state-space model is:
+      //
+      //   dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
+      //   A = -G²Kₜ/(KᵥRJ)
+      //   B = GKₜ/(RJ)
+      //
+      // Solve for G.
+      //
+      //   A/B = -G/Kᵥ
+      //   G = -KᵥA/B
+      //
+      // Solve for J.
+      //
+      //   B = GKₜ/(RJ)
+      //   J = GKₜ/(RB)
+      m_gearing(-gearbox.Kv.value() * m_plant.A(1, 1) / m_plant.B(1, 0)),
+      m_j(m_gearing * gearbox.Kt.value() /
+          (gearbox.R.value() * m_plant.B(1, 0))) {}
 
 void DCMotorSim::SetState(units::radian_t angularPosition,
                           units::radians_per_second_t angularVelocity) {
@@ -37,6 +50,15 @@ units::radians_per_second_t DCMotorSim::GetAngularVelocity() const {
   return units::radians_per_second_t{GetOutput(1)};
 }
 
+units::radians_per_second_squared_t DCMotorSim::GetAngularAcceleration() const {
+  return units::radians_per_second_squared_t{
+      (m_plant.A() * m_x + m_plant.B() * m_u)(0, 0)};
+}
+
+units::newton_meter_t DCMotorSim::GetTorque() const {
+  return units::newton_meter_t{GetAngularAcceleration().value() * m_j.value()};
+}
+
 units::ampere_t DCMotorSim::GetCurrentDraw() const {
   // I = V / R - omega / (Kv * R)
   // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
@@ -46,6 +68,10 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const {
          wpi::sgn(m_u(0));
 }
 
+units::volt_t DCMotorSim::GetInputVoltage() const {
+  return units::volt_t{GetInput(0)};
+}
+
 void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
   SetInput(Vectord<1>{voltage.value()});
   ClampInput(frc::RobotController::GetBatteryVoltage().value());

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

@@ -5,8 +5,10 @@
 #pragma once
 
 #include <units/angle.h>
+#include <units/angular_acceleration.h>
 #include <units/angular_velocity.h>
 #include <units/moment_of_inertia.h>
+#include <units/torque.h>
 
 #include "frc/simulation/LinearSystemSim.h"
 #include "frc/system/LinearSystem.h"
@@ -27,26 +29,9 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> {
    * radians.
    * @param gearbox            The type of and number of motors in the DC motor
    * gearbox.
-   * @param gearing            The gearing of the DC motor (numbers greater than
-   * 1 represent reductions).
    * @param measurementStdDevs The standard deviation of the measurement noise.
    */
   DCMotorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
-             double gearing,
-             const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
-
-  /**
-   * Creates a simulated DC motor mechanism.
-   *
-   * @param gearbox            The type of and number of motors in the DC motor
-   * gearbox.
-   * @param gearing            The gearing of the DC motor (numbers greater than
-   * 1 represent reductions).
-   * @param moi                The moment of inertia of the DC motor.
-   * @param measurementStdDevs The standard deviation of the measurement noise.
-   */
-  DCMotorSim(const DCMotor& gearbox, double gearing,
-             units::kilogram_square_meter_t moi,
              const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
 
   using LinearSystemSim::SetState;
@@ -74,22 +59,59 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> {
    */
   units::radians_per_second_t GetAngularVelocity() const;
 
+  /**
+   * Returns the DC motor acceleration.
+   *
+   * @return The DC motor acceleration
+   */
+  units::radians_per_second_squared_t GetAngularAcceleration() const;
+
+  /**
+   * Returns the DC motor torque.
+   *
+   * @return The DC motor torque
+   */
+  units::newton_meter_t GetTorque() const;
+
   /**
    * Returns the DC motor current draw.
    *
    * @return The DC motor current draw.
    */
   units::ampere_t GetCurrentDraw() const;
 
+  /**
+   * Gets the input voltage for the DC motor.
+   *
+   * @return The DC motor input voltage.
+   */
+  units::volt_t GetInputVoltage() const;
+
   /**
    * Sets the input voltage for the DC motor.
    *
    * @param voltage The input voltage.
    */
   void SetInputVoltage(units::volt_t voltage);
 
+  /**
+   * Returns the gearbox.
+   */
+  const DCMotor& Gearbox() const { return m_gearbox; }
+
+  /**
+   * Returns the gearing;
+   */
+  double Gearing() const { return m_gearing; }
+
+  /**
+   * Returns the moment of inertia
+   */
+  units::kilogram_square_meter_t J() const { return m_j; }
+
  private:
   DCMotor m_gearbox;
   double m_gearing;
+  units::kilogram_square_meter_t m_j;
 };
 }  // namespace frc::sim

wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp

@@ -12,11 +12,13 @@
 #include "frc/simulation/DCMotorSim.h"
 #include "frc/simulation/EncoderSim.h"
 #include "frc/simulation/RoboRioSim.h"
+#include "frc/system/plant/LinearSystemId.h"
 
 TEST(DCMotorSimTest, VoltageSteadyState) {
   frc::DCMotor gearbox = frc::DCMotor::NEO(1);
-  frc::sim::DCMotorSim sim{gearbox, 1.0,
-                           units::kilogram_square_meter_t{0.0005}};
+  auto plant = frc::LinearSystemId::DCMotorSystem(
+      frc::DCMotor::NEO(1), units::kilogram_square_meter_t{0.0005}, 1.0);
+  frc::sim::DCMotorSim sim{plant, gearbox};
 
   frc::Encoder encoder{0, 1};
   frc::sim::EncoderSim encoderSim{encoder};
@@ -60,8 +62,9 @@ TEST(DCMotorSimTest, VoltageSteadyState) {
 
 TEST(DCMotorSimTest, PositionFeedbackControl) {
   frc::DCMotor gearbox = frc::DCMotor::NEO(1);
-  frc::sim::DCMotorSim sim{gearbox, 1.0,
-                           units::kilogram_square_meter_t{0.0005}};
+  auto plant = frc::LinearSystemId::DCMotorSystem(
+      frc::DCMotor::NEO(1), units::kilogram_square_meter_t{0.0005}, 1.0);
+  frc::sim::DCMotorSim sim{plant, gearbox};
 
   frc::PIDController controller{0.04, 0.0, 0.001};