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
@@ -85,29 +85,6 @@ public class TrackTarget extends CommandBase {
//m_turret.runshooterRotatePID(m_targetAngle);
}
/* public static double angleToCenter(double x, double y, double gyro) {
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
return angle;
}*/
/**
* Checks if in hardware deadzone (due to mechanical limitations).
* @param angle Angle to check.
* @return True if in hardware deadzone.
*/
public static boolean isHardwareDeadzone(double angle) {
return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT));
}
/**
* Checks if in digital deadzone (due to climber).
* @param angle Angle to check.
* @return True if in digital deadzone.
*/
public static boolean isDigitalDeadzone(double angle) {
return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT));
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
@@ -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.
*