diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 3a165e591e..ccbf83b3c0 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -39,12 +39,20 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, m_maxHeight(maxHeight), m_gearing(gearing) {} -bool ElevatorSim::HasHitLowerLimit(const Eigen::Matrix& x) const { - return x(0) < m_minHeight.to(); +bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const { + return elevatorHeight < m_minHeight; } -bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix& x) const { - return x(0) > m_maxHeight.to(); +bool ElevatorSim::WouldHitUpperLimit(units::meter_t elevatorHeight) const { + return elevatorHeight > m_maxHeight; +} + +bool ElevatorSim::HasHitLowerLimit() const { + return WouldHitLowerLimit(units::meter_t(m_y(0))); +} + +bool ElevatorSim::HasHitUpperLimit() const { + return WouldHitUpperLimit(units::meter_t(m_y(0))); } units::meter_t ElevatorSim::GetPosition() const { @@ -85,10 +93,10 @@ Eigen::Matrix ElevatorSim::UpdateX( }, currentXhat, u, dt); // Check for collision after updating x-hat. - if (HasHitLowerLimit(updatedXhat)) { + if (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) { return MakeMatrix<2, 1>(m_minHeight.to(), 0.0); } - if (HasHitUpperLimit(updatedXhat)) { + if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) { return MakeMatrix<2, 1>(m_maxHeight.to(), 0.0); } return updatedXhat; diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index 926a18a57f..3b1e943b9b 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -39,14 +39,20 @@ SingleJointedArmSim::SingleJointedArmSim( gearbox, gearing, armLength, minAngle, maxAngle, mass, simulateGravity, measurementStdDevs) {} -bool SingleJointedArmSim::HasHitLowerLimit( - const Eigen::Matrix& x) const { - return x(0) < m_minAngle.to(); +bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const { + return armAngle < m_minAngle; } -bool SingleJointedArmSim::HasHitUpperLimit( - const Eigen::Matrix& x) const { - return x(0) > m_maxAngle.to(); +bool SingleJointedArmSim::WouldHitUpperLimit(units::radian_t armAngle) const { + return armAngle > m_maxAngle; +} + +bool SingleJointedArmSim::HasHitLowerLimit() const { + return WouldHitLowerLimit(units::radian_t(m_y(0))); +} + +bool SingleJointedArmSim::HasHitUpperLimit() const { + return WouldHitUpperLimit(units::radian_t(m_y(0))); } units::radian_t SingleJointedArmSim::GetAngle() const { @@ -96,9 +102,9 @@ Eigen::Matrix SingleJointedArmSim::UpdateX( currentXhat, u, dt); // Check for collisions. - if (HasHitLowerLimit(updatedXhat)) { + if (WouldHitLowerLimit(units::radian_t(updatedXhat(0)))) { return MakeMatrix<2, 1>(m_minAngle.to(), 0.0); - } else if (HasHitUpperLimit(updatedXhat)) { + } else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) { return MakeMatrix<2, 1>(m_maxAngle.to(), 0.0); } return updatedXhat; diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index fdb25551aa..c7849452e1 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -57,21 +57,35 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { units::meter_t minHeight, units::meter_t maxHeight, const std::array& measurementStdDevs = {0.0}); + /** + * Returns whether the elevator would hit the lower limit. + * + * @param elevatorHeight The elevator height. + * @return Whether the elevator would hit the lower limit. + */ + bool WouldHitLowerLimit(units::meter_t elevatorHeight) const; + + /** + * Returns whether the elevator would hit the upper limit. + * + * @param elevatorHeight The elevator height. + * @return Whether the elevator would hit the upper limit. + */ + bool WouldHitUpperLimit(units::meter_t elevatorHeight) const; + /** * Returns whether the elevator has hit the lower limit. * - * @param x The current elevator state. * @return Whether the elevator has hit the lower limit. */ - bool HasHitLowerLimit(const Eigen::Matrix& x) const; + bool HasHitLowerLimit() const; /** * Returns whether the elevator has hit the upper limit. * - * @param x The current elevator state. * @return Whether the elevator has hit the upper limit. */ - bool HasHitUpperLimit(const Eigen::Matrix& x) const; + bool HasHitUpperLimit() const; /** * Returns the position of the elevator. diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 73d8bd1e0b..3fe42ba0fb 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -62,21 +62,35 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { bool simulateGravity, const std::array& measurementStdDevs = {0.0}); + /** + * Returns whether the arm would hit the lower limit. + * + * @param armAngle The arm height. + * @return Whether the arm would hit the lower limit. + */ + bool WouldHitLowerLimit(units::radian_t armAngle) const; + + /** + * Returns whether the arm would hit the upper limit. + * + * @param armAngle The arm height. + * @return Whether the arm would hit the upper limit. + */ + bool WouldHitUpperLimit(units::radian_t armAngle) const; + /** * Returns whether the arm has hit the lower limit. * - * @param x The current arm state. * @return Whether the arm has hit the lower limit. */ - bool HasHitLowerLimit(const Eigen::Matrix& x) const; + bool HasHitLowerLimit() const; /** * Returns whether the arm has hit the upper limit. * - * @param x The current arm state. * @return Whether the arm has hit the upper limit. */ - bool HasHitUpperLimit(const Eigen::Matrix& x) const; + bool HasHitUpperLimit() const; /** * Returns the current arm angle. @@ -100,7 +114,7 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { units::ampere_t GetCurrentDraw() const override; /** - * Sets the input voltage for the elevator. + * Sets the input voltage for the arm. * * @param voltage The input voltage. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 12f2506362..cc271e21a5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -127,26 +127,42 @@ public class ElevatorSim extends LinearSystemSim { m_maxHeight = maxHeightMeters; } + /** + * Returns whether the elevator would hit the lower limit. + * + * @param elevatorHeightMeters The elevator height. + * @return Whether the elevator would hit the lower limit. + */ + public boolean wouldHitLowerLimit(double elevatorHeightMeters) { + return elevatorHeightMeters < this.m_minHeight; + } + + /** + * Returns whether the elevator would hit the upper limit. + * + * @param elevatorHeightMeters The elevator height. + * @return Whether the elevator would hit the upper limit. + */ + public boolean wouldHitUpperLimit(double elevatorHeightMeters) { + return elevatorHeightMeters > this.m_maxHeight; + } + /** * Returns whether the elevator has hit the lower limit. * - * @param x The current elevator state. * @return Whether the elevator has hit the lower limit. */ - @SuppressWarnings("ParameterName") - public boolean hasHitLowerLimit(Matrix x) { - return x.get(0, 0) < this.m_minHeight; + public boolean hasHitLowerLimit() { + return wouldHitLowerLimit(getPositionMeters()); } /** * Returns whether the elevator has hit the upper limit. * - * @param x The current elevator state. * @return Whether the elevator has hit the upper limit. */ - @SuppressWarnings("ParameterName") - public boolean hasHitUpperLimit(Matrix x) { - return x.get(0, 0) > this.m_maxHeight; + public boolean hasHitUpperLimit() { + return wouldHitUpperLimit(getPositionMeters()); } /** @@ -215,10 +231,10 @@ public class ElevatorSim extends LinearSystemSim { dtSeconds); // We check for collisions after updating x-hat. - if (hasHitLowerLimit(updatedXhat)) { + if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { return VecBuilder.fill(m_minHeight, 0); } - if (hasHitUpperLimit(updatedXhat)) { + if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { return VecBuilder.fill(m_maxHeight, 0); } return updatedXhat; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index 2a09d7d84f..c0ecc9bede 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -173,26 +173,42 @@ public class SingleJointedArmSim extends LinearSystemSim { m_simulateGravity = simulateGravity; } + /** + * Returns whether the arm would hit the lower limit. + * + * @param currentAngleRads The current arm height. + * @return Whether the arm would hit the lower limit. + */ + public boolean wouldHitLowerLimit(double currentAngleRads) { + return currentAngleRads < this.m_minAngle; + } + + /** + * Returns whether the arm would hit the upper limit. + * + * @param currentAngleRads The current arm height. + * @return Whether the arm would hit the upper limit. + */ + public boolean wouldHitUpperLimit(double currentAngleRads) { + return currentAngleRads > this.m_maxAngle; + } + /** * Returns whether the arm has hit the lower limit. * - * @param x The current arm state. * @return Whether the arm has hit the lower limit. */ - @SuppressWarnings("ParameterName") - public boolean hasHitLowerLimit(Matrix x) { - return x.get(0, 0) < this.m_minAngle; + public boolean hasHitLowerLimit() { + return wouldHitLowerLimit(getAngleRads()); } /** * Returns whether the arm has hit the upper limit. * - * @param x The current arm state. * @return Whether the arm has hit the upper limit. */ - @SuppressWarnings("ParameterName") - public boolean hasHitUpperLimit(Matrix x) { - return x.get(0, 0) > this.m_maxAngle; + public boolean hasHitUpperLimit() { + return wouldHitUpperLimit(getAngleRads()); } /** @@ -227,7 +243,7 @@ public class SingleJointedArmSim extends LinearSystemSim { } /** - * Sets the input voltage for the elevator. + * Sets the input voltage for the arm. * * @param volts The input voltage. */ @@ -288,10 +304,10 @@ public class SingleJointedArmSim extends LinearSystemSim { dtSeconds); // We check for collision after updating xhat - if (hasHitLowerLimit(updatedXhat)) { + if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { return VecBuilder.fill(m_minAngle, 0); } - if (hasHitUpperLimit(updatedXhat)) { + if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { return VecBuilder.fill(m_maxAngle, 0); } return updatedXhat;