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] Allow disabling ElevatorSim gravity (#4145)

Browse Source 
Closes #4144.
This commit is contained in:
Tyler Veness
2022-04-24 07:19:18 -07:00
committed by GitHub
parent aaa69f6717
commit 9d20ab3024
9 changed files with 97 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -15,19 +15,20 @@ using namespace frc::sim;
ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
const DCMotor& gearbox, double gearing,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight,
units::meter_t maxHeight, bool simulateGravity,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim(plant, measurementStdDevs),
m_gearbox(gearbox),
m_drumRadius(drumRadius),
m_minHeight(minHeight),
m_maxHeight(maxHeight),
m_gearing(gearing) {}
m_gearing(gearing),
m_simulateGravity(simulateGravity) {}
ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
units::kilogram_t carriageMass,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight,
units::meter_t maxHeight, bool simulateGravity,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
@@ -36,7 +37,8 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
m_drumRadius(drumRadius),
m_minHeight(minHeight),
m_maxHeight(maxHeight),
m_gearing(gearing) {}
m_gearing(gearing),
m_simulateGravity(simulateGravity) {}
bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const {
return elevatorHeight < m_minHeight;
@@ -87,8 +89,12 @@ Eigen::Vector<double, 2> ElevatorSim::UpdateX(
auto updatedXhat = RKDP(
[&](const Eigen::Vector<double, 2>& x,
const Eigen::Vector<double, 1>& u_) -> Eigen::Vector<double, 2> {
return m_plant.A() * x + m_plant.B() * u_ +
Eigen::Vector<double, 2>{0.0, -9.8};
Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{0.0, -9.8};
}
return xdot;
},
currentXhat, u, dt);
// Check for collision after updating x-hat.

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

@@ -18,13 +18,13 @@ using namespace frc::sim;
SingleJointedArmSim::SingleJointedArmSim(
const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity,
units::radian_t maxAngle, units::kilogram_t armMass, bool simulateGravity,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
m_r(armLength),
m_minAngle(minAngle),
m_maxAngle(maxAngle),
m_mass(mass),
m_armMass(armMass),
m_gearbox(gearbox),
m_gearing(gearing),
m_simulateGravity(simulateGravity) {}
@@ -32,11 +32,11 @@ SingleJointedArmSim::SingleJointedArmSim(
SingleJointedArmSim::SingleJointedArmSim(
const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity,
units::radian_t maxAngle, units::kilogram_t armMass, bool simulateGravity,
const std::array<double, 1>& measurementStdDevs)
: SingleJointedArmSim(
LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
gearbox, gearing, armLength, minAngle, maxAngle, mass,
gearbox, gearing, armLength, minAngle, maxAngle, armMass,
simulateGravity, measurementStdDevs) {}
bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const {
@@ -94,7 +94,7 @@ Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{
0.0, (m_mass * m_r * -9.8 * 3.0 / (m_mass * m_r * m_r) *
0.0, (m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) *
std::cos(x(0)))
.value()};
}