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] Fix DifferentialDrivetrainSimTest (#8934)

Browse Source 
- Fix a dependency on other test global state via Vin by calling
ResetData() to ensure consistent state
- Clamp the ground truth voltages to match sim.SetInputs()
- The C++ test was checking the noisy measurement output instead of
using sim.GetState()
- Make GetState() public in C++ and Java
This commit is contained in:
Peter Johnson
2026-06-21 20:20:21 -07:00
committed by GitHub
parent be110d0469
commit aef281f43d
5 changed files with 44 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,11 +14,16 @@
#include "wpi/math/system/NumericalIntegration.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp"
#include "wpi/simulation/RoboRioSim.hpp"
#include "wpi/units/current.hpp"
#include "wpi/units/math.hpp"
#include "wpi/units/moment_of_inertia.hpp"
TEST(DifferentialDrivetrainSimTest, Convergence) {
using State = wpi::sim::DifferentialDrivetrainSim::State;
wpi::sim::RoboRioSim::ResetData();
auto motor = wpi::math::DCMotor::NEO(2);
auto plant = wpi::math::Models::DifferentialDriveFromPhysicalConstants(
motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
@@ -50,10 +55,11 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
auto [l, r] = kinematics.ToWheelVelocities(feedbackOut);
auto voltages =
feedforward.Calculate(wpi::math::Vectord<2>{l.value(), r.value()});
auto clampedVoltages = sim.ClampInput(voltages);
// Sim periodic code.
sim.SetInputs(wpi::units::volt_t{voltages(0, 0)},
wpi::units::volt_t{voltages(1, 0)});
sim.SetInputs(wpi::units::volt_t{clampedVoltages(0, 0)},
wpi::units::volt_t{clampedVoltages(1, 0)});
sim.Update(20_ms);
// Update ground truth.
@@ -61,14 +67,14 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
[&sim](const auto& x, const auto& u) -> wpi::math::Vectord<7> {
return sim.Dynamics(x, u);
},
groundTruthX, voltages, 20_ms);
groundTruthX, clampedVoltages, 20_ms);
}
// 2 inch tolerance is OK since our ground truth is an approximation of the
// ODE solution using wpi::math::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);
EXPECT_NEAR(groundTruthX(0, 0), sim.GetState(State::X), 0.05);
EXPECT_NEAR(groundTruthX(1, 0), sim.GetState(State::Y), 0.05);
EXPECT_NEAR(groundTruthX(2, 0), sim.GetState(State::HEADING), 0.01);
}
TEST(DifferentialDrivetrainSimTest, Current) {