Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

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;