From c3fd20db592b52869aebfe7a18537054d0e4a2a5 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 22 Dec 2021 22:27:51 -0800
Subject: [PATCH] [wpilib] Fix trajectory sampling in DifferentialDriveSim test
 (#3821)

Also rename C++ test file to match class name.

Fixes #3818.
---
 ...eSimTest.cpp => DifferentialDrivetrainSimTest.cpp} | 11 +++++------
 .../simulation/DifferentialDrivetrainSimTest.java     |  2 +-
 2 files changed, 6 insertions(+), 7 deletions(-)
 rename wpilibc/src/test/native/cpp/simulation/{DifferentialDriveSimTest.cpp => DifferentialDrivetrainSimTest.cpp} (93%)

diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
similarity index 93%
rename from wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
rename to wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
index eabbecb367..8ccfcd60b2 100644
--- a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
@@ -17,7 +17,7 @@
 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
 #include "gtest/gtest.h"
 
-TEST(DifferentialDriveSimTest, Convergence) {
+TEST(DifferentialDrivetrainSimTest, Convergence) {
   auto motor = frc::DCMotor::NEO(2);
   auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
       motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
@@ -42,9 +42,8 @@ TEST(DifferentialDriveSimTest, Convergence) {
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       frc::Pose2d(), {}, frc::Pose2d(2_m, 2_m, 0_rad), config);
 
-  // NOLINTNEXTLINE
-  for (double t = 0; t < trajectory.TotalTime().value(); t += 0.02) {
-    auto state = trajectory.Sample(20_ms);
+  for (auto t = 0_s; t < trajectory.TotalTime(); t += 20_ms) {
+    auto state = trajectory.Sample(t);
     auto ramseteOut = ramsete.Calculate(sim.GetPose(), state);
 
     auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
@@ -70,7 +69,7 @@ TEST(DifferentialDriveSimTest, Convergence) {
   EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);
 }
 
-TEST(DifferentialDriveSimTest, Current) {
+TEST(DifferentialDrivetrainSimTest, Current) {
   auto motor = frc::DCMotor::NEO(2);
   auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
       motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
@@ -97,7 +96,7 @@ TEST(DifferentialDriveSimTest, Current) {
   EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
 }
 
-TEST(DifferentialDriveSimTest, ModelStability) {
+TEST(DifferentialDrivetrainSimTest, ModelStability) {
   auto motor = frc::DCMotor::NEO(2);
   auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
       motor, 50_kg, 2_in, 12_in, 2_kg_sq_m, 5.0);
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
index 52c446c404..5baed3ab15 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
@@ -62,7 +62,7 @@ class DifferentialDrivetrainSimTest {
                 .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
 
     for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) {
-      var state = traj.sample(0.020);
+      var state = traj.sample(t);
       var ramseteOut = ramsete.calculate(sim.getPose(), state);
 
       var wheelSpeeds = kinematics.toWheelSpeeds(ramseteOut);