diff --git a/build.gradle b/build.gradle index 53d493b6..2d32394c 100644 --- a/build.gradle +++ b/build.gradle @@ -100,7 +100,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:quick-sort-SNAPSHOT' + implementation 'com.github.Programming-TRIGON:TRIGONLib:ca5e770f3d' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index a866c425..8227b6d3 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -166,7 +166,7 @@ private void updateFromVision() { } private void sortCamerasByLastTargetTimestamp(AprilTagCamera[] aprilTagCameras) { - QuickSort.sort(aprilTagCameras, (aprilTagCamera) -> ((AprilTagCamera) aprilTagCamera).getLatestResultTimestampSeconds()); + QuickSort.sort(aprilTagCameras, AprilTagCamera::getLatestResultTimestampSeconds); } /** @@ -236,7 +236,7 @@ private Transform2d calculatePoseAmbiguity(Pose2d estimatedPoseAtObservationTime final PoseEstimatorConstants.StandardDeviations estimatedPoseStandardDeviations = cameraStandardDeviations.combineOdometryAndVisionStandardDeviations(); return scaleTransformFromStandardDeviations(poseEstimateAtObservationTimeToObservationPose, estimatedPoseStandardDeviations); } - + private Transform2d scaleTransformFromStandardDeviations(Transform2d transform, PoseEstimatorConstants.StandardDeviations standardDeviations) { return new Transform2d( transform.getX() * standardDeviations.translation(),