From 2ddc9fe2fc07fe43ac9cf0341bc7f55fb7d76b58 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 21 Jan 2022 17:23:08 -0700 Subject: [PATCH] distance between poses function --- .../java/frc4388/robot/subsystems/SwerveDrive.java | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ae7f81b..9eed60a 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -157,6 +157,16 @@ public class SwerveDrive extends SubsystemBase { estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0)))); } + /** + * Gets the distance between two given poses. + * @param p1 The first pose. + * @param p2 The second pose. + * @return Absolute distance between p1 and p2. + */ + public double distBtwPoses(Pose2d p1, Pose2d p2) { + return Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2)); + } + /** * Returns a scalar from your distance to the hub to your target distance. * @@ -167,9 +177,7 @@ public class SwerveDrive extends SubsystemBase { Pose2d p1 = m_poseEstimator.getEstimatedPosition(); Pose2d p2 = SwerveDriveConstants.HUB_POSE; - double dist = Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2)); - - return target_dist/dist; + return target_dist/distBtwPoses(p1, p2); } /** Updates the field relative position of the robot. */