deleting unused stuff

This commit is contained in:
aarav18
2022-03-06 15:31:39 -07:00
parent 2ab6e0a8db
commit 5c7defd193
2 changed files with 0 additions and 51 deletions
@@ -184,34 +184,6 @@ public class SwerveDrive extends SubsystemBase {
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
/**
* 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.
*
* @param target_dist The target distance.
* @return A scalar that multiplies your distance from the hub to get your
* target distance.
*/
public Pose2d poseGivenDist(double target_dist) {
Pose2d p1 = m_poseEstimator.getEstimatedPosition();
Pose2d p2 = SwerveDriveConstants.HUB_POSE;
double scalar = target_dist / distBtwPoses(p1, p2);
Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation());
return new_pose;
}
/**
* Gets the current pose of the robot.
*