Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Fixup wouldHitLowerLimit in elevator and arm simulation classes. (#3076)

Browse Source 
Closes #3050.
This commit is contained in:
Matt
2021-01-14 00:28:00 -08:00
committed by GitHub
parent 04a90b5dd1
commit 406d055f07
6 changed files with 118 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -39,12 +39,20 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
m_maxHeight(maxHeight),
m_gearing(gearing) {}
bool ElevatorSim::HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) < m_minHeight.to<double>();
bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const {
return elevatorHeight < m_minHeight;
}
bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) > m_maxHeight.to<double>();
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<double, 2, 1> 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<double>(), 0.0);
}
if (HasHitUpperLimit(updatedXhat)) {
if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) {
return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
}
return updatedXhat;

22
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -39,14 +39,20 @@ SingleJointedArmSim::SingleJointedArmSim(
gearbox, gearing, armLength, minAngle, maxAngle, mass,
simulateGravity, measurementStdDevs) {}
bool SingleJointedArmSim::HasHitLowerLimit(
const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) < m_minAngle.to<double>();
bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const {
return armAngle < m_minAngle;
}
bool SingleJointedArmSim::HasHitUpperLimit(
const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) > m_maxAngle.to<double>();
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<double, 2, 1> 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<double>(), 0.0);
} else if (HasHitUpperLimit(updatedXhat)) {
} else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) {
return MakeMatrix<2, 1>(m_maxAngle.to<double>(), 0.0);
}
return updatedXhat;

22
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
View File

@@ -57,21 +57,35 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
units::meter_t minHeight, units::meter_t maxHeight,
const std::array<double, 1>& 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<double, 2, 1>& 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<double, 2, 1>& x) const;
bool HasHitUpperLimit() const;
/**
* Returns the position of the elevator.

24
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
View File

@@ -62,21 +62,35 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
bool simulateGravity,
const std::array<double, 1>& 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<double, 2, 1>& 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<double, 2, 1>& 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.
*/

36
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -127,26 +127,42 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
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<N2, N1> 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<N2, N1> x) {
return x.get(0, 0) > this.m_maxHeight;
public boolean hasHitUpperLimit() {
return wouldHitUpperLimit(getPositionMeters());
}
/**
@@ -215,10 +231,10 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
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;

38
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -173,26 +173,42 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
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<N2, N1> 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<N2, N1> x) {
return x.get(0, 0) > this.m_maxAngle;
public boolean hasHitUpperLimit() {
return wouldHitUpperLimit(getAngleRads());
}
/**
@@ -227,7 +243,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
}
/**
* 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<N2, N1, N1> {
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;