diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java index b23ee1ec0e..faf55634ad 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java @@ -231,7 +231,6 @@ class DifferentialDriveAccelerationLimiterTest { .times(xAccelLimiter) .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; - System.out.println(a); assertTrue(Math.abs(a) > maxA); assertTrue(Math.abs(a) > -minA); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java index 84ba7473b6..a353bc94a9 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java @@ -143,8 +143,6 @@ class DifferentialDrivePoseEstimatorTest { double t = 0.0; - System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n"); - final TreeMap visionUpdateQueue = new TreeMap<>(); double maxError = Double.NEGATIVE_INFINITY; @@ -191,16 +189,6 @@ class DifferentialDrivePoseEstimatorTest { leftDistanceMeters, rightDistanceMeters); - System.out.printf( - "%f, %f, %f, %f, %f, %f, %f\n", - t, - xHat.getX(), - xHat.getY(), - xHat.getRotation().getRadians(), - groundTruthState.poseMeters.getX(), - groundTruthState.poseMeters.getY(), - groundTruthState.poseMeters.getRotation().getRadians()); - double error = groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java index fcaebb7eba..3c99e91e52 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java @@ -151,8 +151,6 @@ class MecanumDrivePoseEstimatorTest { double t = 0.0; - System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n"); - final TreeMap visionUpdateQueue = new TreeMap<>(); double maxError = Double.NEGATIVE_INFINITY; @@ -200,16 +198,6 @@ class MecanumDrivePoseEstimatorTest { .minus(trajectory.getInitialPose().getRotation()), wheelPositions); - System.out.printf( - "%f, %f, %f, %f, %f, %f, %f\n", - t, - xHat.getX(), - xHat.getY(), - xHat.getRotation().getRadians(), - groundTruthState.poseMeters.getX(), - groundTruthState.poseMeters.getY(), - groundTruthState.poseMeters.getRotation().getRadians()); - double error = groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java index 49679d4a3b..c4ac274c09 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -165,11 +165,6 @@ class SwerveDrivePoseEstimatorTest { double t = 0.0; - System.out.print( - "time, est_x, est_y, est_theta, true_x, true_y, true_theta, " - + "distance_1, distance_2, distance_3, distance_4, " - + "angle_1, angle_2, angle_3, angle_4\n"); - final TreeMap visionUpdateQueue = new TreeMap<>(); double maxError = Double.NEGATIVE_INFINITY; @@ -219,24 +214,6 @@ class SwerveDrivePoseEstimatorTest { .minus(trajectory.getInitialPose().getRotation()), positions); - System.out.printf( - "%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n", - t, - xHat.getX(), - xHat.getY(), - xHat.getRotation().getRadians(), - groundTruthState.poseMeters.getX(), - groundTruthState.poseMeters.getY(), - groundTruthState.poseMeters.getRotation().getRadians(), - positions[0].distanceMeters, - positions[1].distanceMeters, - positions[2].distanceMeters, - positions[3].distanceMeters, - positions[0].angle.getRadians(), - positions[1].angle.getRadians(), - positions[2].angle.getRadians(), - positions[3].angle.getRadians()); - double error = groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) {