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