[wpilib] Clamp input voltage in sim classes (#2955)

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

@@ -6,6 +6,7 @@
 
 #include <frc/system/plant/LinearSystemId.h>
 
+#include "frc/RobotController.h"
 #include "frc/system/RungeKutta.h"
 
 using namespace frc;
@@ -36,9 +37,16 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
               driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
           trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
 
+Eigen::Matrix<double, 2, 1> DifferentialDrivetrainSim::ClampInput(
+    Eigen::Matrix<double, 2, 1> u) {
+  return frc::NormalizeInputVector<2>(u,
+                                      frc::RobotController::GetInputVoltage());
+}
+
 void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
                                           units::volt_t rightVoltage) {
   m_u << leftVoltage.to<double>(), rightVoltage.to<double>();
+  m_u = ClampInput(m_u);
 }
 
 void DifferentialDrivetrainSim::SetGearing(double newGearing) {

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

@@ -72,6 +72,16 @@ class DifferentialDrivetrainSim {
       units::meter_t trackWidth,
       const std::array<double, 7>& measurementStdDevs = {});
 
+  /**
+   * Clamp the input vector such that no element exceeds the given voltage. If
+   * any does, the relative magnitudes of the input will be maintained.
+   *
+   * @param u          The input vector.
+   * @param maxVoltage The maximum voltage.
+   * @return The normalized input.
+   */
+  Eigen::Matrix<double, 2, 1> ClampInput(Eigen::Matrix<double, 2, 1> u);
+
   /**
    * Sets the applied voltage to the drivetrain. Note that positive voltage must
    * make that side of the drivetrain travel forward (+X).

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

@@ -10,6 +10,7 @@
 #include <units/current.h>
 #include <units/time.h>
 
+#include "frc/RobotController.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/LinearSystem.h"
 
@@ -82,7 +83,9 @@ class LinearSystemSim {
    *
    * @param u The system inputs.
    */
-  void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
+  void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) {
+    m_u = ClampInput(u);
+  }
 
   /*
    * Sets the system inputs.
@@ -90,7 +93,10 @@ class LinearSystemSim {
    * @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; }
+  void SetInput(int row, double value) {
+    m_u(row, 0) = value;
+    ClampInput(m_u);
+  }
 
   /**
    * Sets the system state.
@@ -123,6 +129,19 @@ class LinearSystemSim {
     return m_plant.CalculateX(currentXhat, u, dt);
   }
 
+  /**
+   * Clamp the input vector such that no element exceeds the given voltage. If
+   * any does, the relative magnitudes of the input will be maintained.
+   *
+   * @param u          The input vector.
+   * @return The normalized input.
+   */
+  Eigen::Matrix<double, Inputs, 1> ClampInput(
+      Eigen::Matrix<double, Inputs, 1> u) {
+    return frc::NormalizeInputVector<Inputs>(
+        u, frc::RobotController::GetInputVoltage());
+  }
+
   LinearSystem<States, Inputs, Outputs> m_plant;
 
   Eigen::Matrix<double, States, 1> m_x;

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.math.StateSpaceUtil;
@@ -121,7 +122,7 @@ public class DifferentialDrivetrainSim {
    * @param rightVoltageVolts The right voltage.
    */
   public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
-    m_u = VecBuilder.fill(leftVoltageVolts, rightVoltageVolts);
+    m_u = clampInput(VecBuilder.fill(leftVoltageVolts, rightVoltageVolts));
   }
 
   @SuppressWarnings("LocalVariableName")
@@ -278,6 +279,17 @@ public class DifferentialDrivetrainSim {
     return xdot;
   }
 
+  /**
+   * Clamp the input vector such that no element exceeds the given voltage. If any does,
+   * the relative magnitudes of the input will be maintained.
+   *
+   * @param u          The input vector.
+   * @return The normalized input.
+   */
+  protected Matrix<N2, N1> clampInput(Matrix<N2, N1> u) {
+    return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
+  }
+
   enum State {
     kX(0),
     kY(1),

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
+import edu.wpi.first.wpilibj.RobotController;
 import org.ejml.MatrixDimensionException;
 import org.ejml.simple.SimpleMatrix;
 
@@ -113,7 +114,7 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
    * @param u The system inputs.
    */
   public void setInput(Matrix<Inputs, N1> u) {
-    this.m_u = u;
+    this.m_u = clampInput(u);
   }
 
   /**
@@ -124,6 +125,7 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
    */
   public void setInput(int row, double value) {
     m_u.set(row, 0, value);
+    m_u = clampInput(m_u);
   }
 
   /**
@@ -170,4 +172,15 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
                                        Matrix<Inputs, N1> u, double dtSeconds) {
     return m_plant.calculateX(currentXhat, u, dtSeconds);
   }
+
+  /**
+   * Clamp the input vector such that no element exceeds the given voltage. If any does,
+   * the relative magnitudes of the input will be maintained.
+   *
+   * @param u          The input vector.
+   * @return The normalized input.
+   */
+  protected Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> u) {
+    return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
+  }
 }