diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 0a45e62616..c11f34845f 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -6,6 +6,7 @@ #include +#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 DifferentialDrivetrainSim::ClampInput( + Eigen::Matrix u) { + return frc::NormalizeInputVector<2>(u, + frc::RobotController::GetInputVoltage()); +} + void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage) { m_u << leftVoltage.to(), rightVoltage.to(); + m_u = ClampInput(m_u); } void DifferentialDrivetrainSim::SetGearing(double newGearing) { diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h index c4c27bf85e..30b5ce4f58 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h @@ -72,6 +72,16 @@ class DifferentialDrivetrainSim { units::meter_t trackWidth, const std::array& 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 ClampInput(Eigen::Matrix u); + /** * Sets the applied voltage to the drivetrain. Note that positive voltage must * make that side of the drivetrain travel forward (+X). diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h index 585e8eb0de..eb59f48e80 100644 --- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h @@ -10,6 +10,7 @@ #include #include +#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& u) { m_u = u; } + void SetInput(const Eigen::Matrix& 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 ClampInput( + Eigen::Matrix u) { + return frc::NormalizeInputVector( + u, frc::RobotController::GetInputVoltage()); + } + LinearSystem m_plant; Eigen::Matrix m_x; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 15877040ca..b823ffc127 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/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 clampInput(Matrix u) { + return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage()); + } + enum State { kX(0), kY(1), diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java index af1c3921a1..a0261fd897 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java +++ b/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 u) { - this.m_u = u; + this.m_u = clampInput(u); } /** @@ -124,6 +125,7 @@ public class LinearSystemSim 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 clampInput(Matrix u) { + return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage()); + } }