Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

distance between poses function

Browse Source
This commit is contained in:
aarav18
2022-01-21 17:23:08 -07:00
parent 4169a5f8d5
commit 2ddc9fe2fc
1 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+11 -3
View File
@@ -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. */