diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index b809790536..9b0bef467e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -127,11 +127,7 @@ public class LTVDifferentialDriveController { .times(-1.0) .get(0, 0); - var x = new Matrix<>(Nat.N5(), Nat.N1()); for (double velocity = -maxV; velocity < maxV; velocity += 0.01) { - x.set(State.kLeftVelocity.value, 0, velocity); - x.set(State.kRightVelocity.value, 0, velocity); - // The DARE is ill-conditioned if the velocity is close to zero, so don't // let the system stop. if (Math.abs(velocity) < 1e-4) { diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp index cd29dedf1c..a249344607 100644 --- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp @@ -59,11 +59,7 @@ LTVDifferentialDriveController::LTVDifferentialDriveController( units::meters_per_second_t maxV{ -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)}; - Vectord<5> x = Vectord<5>::Zero(); for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) { - x(State::kLeftVelocity) = velocity.value(); - x(State::kRightVelocity) = velocity.value(); - // The DARE is ill-conditioned if the velocity is close to zero, so don't // let the system stop. if (units::math::abs(velocity) < 1e-4_mps) {