moved velocity calculations to the localizers

Pedro-Pathing · Nov 21, 2024 · 089fc3a · 089fc3a
1 parent 9711a26
commit 089fc3a
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/...e/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/...e/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
@@ -262,9 +262,10 @@ public Pose getDeltaPose() {
      */
     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);