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.
*/