Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -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)))); 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. * 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 p1 = m_poseEstimator.getEstimatedPosition();
Pose2d p2 = SwerveDriveConstants.HUB_POSE; 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/distBtwPoses(p1, p2);
return target_dist/dist;
} }
/** Updates the field relative position of the robot. */ /** Updates the field relative position of the robot. */