mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
deleting unused stuff
This commit is contained in:
@@ -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.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user