From 089fc3ae6878b5fe2cbf8764f0df0e4dd1b0c89e Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 21 Nov 2024 17:13:30 -0500 Subject: [PATCH] moved velocity calculations to the localizers --- .../teamcode/pedroPathing/localization/PoseUpdater.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index eff461f..3daf54a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -262,9 +262,10 @@ public class PoseUpdater { */ public Vector getVelocity() { if (currentVelocity == null) { - currentVelocity = new Vector(); - currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY()); - currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); +// currentVelocity = new Vector(); +// currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY()); +// currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); + currentVelocity = localizer.getVelocityVector(); return MathFunctions.copyVector(currentVelocity); } else { return MathFunctions.copyVector(currentVelocity);