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

[wpilib] Use RKDP in DifferentialDrivetrainSim (#5931)

Browse Source
This commit is contained in:
Tyler Veness
2023-11-17 08:41:18 -08:00
committed by GitHub
parent ead9ae5a69
commit b1fad062f7
4 changed files with 6 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -58,7 +58,7 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
}
void DifferentialDrivetrainSim::Update(units::second_t dt) {
m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
m_x = RKDP([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
}

4
wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
View File

@@ -55,7 +55,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
sim.Update(20_ms);
// Update ground truth.
groundTruthX = frc::RK4(
groundTruthX = frc::RKDP(
[&sim](const auto& x, const auto& u) -> frc::Vectord<7> {
return sim.Dynamics(x, u);
},
@@ -63,7 +63,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
}
// 2 inch tolerance is OK since our ground truth is an approximation of the
// ODE solution using RK4 anyway
// ODE solution using RKDP anyway
EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05);
EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05);
EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);