Made angle offsets

src/main/java/frc/robot/RobotContainer.java

@@ -29,6 +29,7 @@
 import frc.robot.commands.swerve.SwerveAngleOffsetCalibration;
 import frc.robot.commands.swerve.TeleopSwerve;
 import frc.robot.commands.swerve.XStance;
+import frc.robot.commands.vision.AprilTagVision;
 import frc.robot.commands.vision.GamePieceVision;
 
 import edu.wpi.first.wpilibj.DriverStation;
@@ -161,8 +162,8 @@ private void configureBindings() {
 
 		driverController.rightTrigger(OperatorConstants.boolTriggerThreshold)
 			.whileTrue(teleopSwerve.holdToggleFieldRelative());
-
-		// right bumper -> rotate to speaker
+		// right bumper -> rotate to speaker apriltag
+		driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve));
 		// left bumper -> rotate to note
 		driverController.leftBumper().onTrue(new GamePieceVision(vision, swerve));
 		driverController.a().onTrue(teleopSwerve.toggleFieldRelative());

src/main/java/frc/robot/commands/arm/AutoAim.java

@@ -40,7 +40,9 @@ public void initialize() {
 
 	@Override
 	public void execute() {
-
+		if (vision.findDistance() > 1)
+			;
+		// add if statements for the range now
 	}
 
 	@Override

src/main/java/frc/robot/commands/vision/AprilTagVision.java

@@ -19,11 +19,15 @@
 
 public class AprilTagVision extends Command {
 	/** Creates a new AprilTagVision. */
-	private Vision vision;
-	private Swerve swerve;
+	private static Vision vision;
+	private static Swerve swerve;
 	private boolean isAligned;
 	private double robotRotate_radps;
 	Optional<Alliance> alliance = DriverStation.getAlliance();
+	public static double rotationalOffset = Math.atan2(VisionConstants.limelightXOffset, vision.findDistance()); // inverse
+																																																								// tangent
+																																																								// of
+																																																								// X/y
 
 	public AprilTagVision(Vision vision, Swerve swerve) {
 		addRequirements(swerve);
@@ -67,9 +71,7 @@ public void execute() {
 		robotRotate_radps *= -1; // Rotate opposite of error
 
 		System.out.println("Swerve --> Shooting Speaker");
-		swerve.drive(new Translation2d(0, 0), robotRotate_radps, Swerve.fieldrelativity); // Swerve.Field Relative is Hacky,
-																																											// janky, and uses global
-																																											// variables.
+		swerve.drive(new Translation2d(0, 0), robotRotate_radps - rotationalOffset, false);
 	}
 
 	// Called once the command ends or is interrupted.

src/main/java/frc/robot/constants/VisionConstants.java

@@ -7,9 +7,19 @@ public class VisionConstants {
 	/* ObjectDetectionVision */
 	public static final double gamePieceRotateKp = 1.2;
 	public static final double gamePieceRotateKd = 0.34;
+
 	public static final double AprilTagRotateKp = 1.2;
 	public static final double AprilTagRotateKd = 0.34;
+
 	public static final double crosshairGamePieceBoundRotateX_rad = Math.toRadians(1.0);
+
 	public static final int redSpeakerTag = 4;
 	public static final int blueSpeakerTag = 7;
+
+	public static final double limelightMountAngleDegrees = 50; // placeholder
+	public static final double LensHeight = 7.5; // placeholder
+	public static final double goalHeight = 53.63; // Speaker tag center height
+
+	public static final double limelightXOffset = 0; // offset the limelight is from the center of the robot
+
 }

src/main/java/frc/robot/io/FieldVisionIO.java

@@ -13,7 +13,7 @@ public static class FieldVisionIOInputs {
 		public int pipeline = 0;
 		public boolean hasTarget = false;
 		public int targetAprilTag = 0;
-		public NetworkTableEntry priorityID;
+		public double ty;
 
 		public double crosshairToTargetErrorX_rad = 0.0;
 		public double crosshairToTargetErrorY_rad = 0.0;
@@ -62,6 +62,7 @@ public void updateInputs(FieldVisionIOInputs inputs) {
 		inputs.crosshairToTargetErrorX_rad = Math.toRadians(txEntry.getDouble(inputs.crosshairToTargetErrorX_rad));
 		inputs.crosshairToTargetErrorY_rad = Math.toRadians(tyEntry.getDouble(inputs.crosshairToTargetErrorX_rad));
 		inputs.targetArea = taEntry.getDouble(inputs.targetArea);
+		inputs.ty = tyEntry.getDouble(inputs.ty);
 
 		targetspaceCache = targetspaceEntry.getDoubleArray(targetspaceCache);
 		if (targetspaceCache.length > 0) {

src/main/java/frc/robot/io/PieceVisionIO.java

@@ -66,7 +66,7 @@ public void updateInputs(PieceVisionIOInputs inputs) {
 		inputs.hasTarget = tvEntry.getDouble(0) == 1;
 		inputs.targetAprilTag = tidEntry.getNumber(inputs.targetAprilTag).intValue();
 		inputs.crosshairToTargetErrorX_rad = Math.toRadians(txEntry.getDouble(inputs.crosshairToTargetErrorX_rad));
-		inputs.crosshairToTargetErrorY_rad = Math.toRadians(tyEntry.getDouble(inputs.crosshairToTargetErrorX_rad));
+		inputs.crosshairToTargetErrorY_rad = Math.toRadians(tyEntry.getDouble(inputs.crosshairToTargetErrorY_rad));
 		inputs.targetArea = taEntry.getDouble(inputs.targetArea);
 
 		targetspaceCache = targetspaceEntry.getDoubleArray(targetspaceCache);

src/main/java/frc/robot/subsystems/Swerve.java

@@ -68,8 +68,6 @@ public class Swerve extends SubsystemBase {
 	private double translationY;
 	private double translationX;
 
-	public static boolean fieldrelativity;
-
 	public Swerve(GyroIO gyroIO, Vision vision) {
 		modules = new SwerveModule[moduleConstants.length];
 		int moduleNumber = 0;
@@ -193,7 +191,6 @@ public void setCenterRotation(double x, double y) {
 	 * for rotation zero is away from the driver, positive is CCW
 	 */
 	public void drive(Translation2d translation_mps, double rotation_radps, boolean fieldRelative) {
-		fieldrelativity = fieldRelative;
 		SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(
 			fieldRelative
 				? ChassisSpeeds.fromFieldRelativeSpeeds(

src/main/java/frc/robot/subsystems/Vision.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import frc.robot.constants.VisionConstants;
 import frc.robot.io.FieldVisionIO;
 import frc.robot.io.FieldVisionIOInputsAutoLogged;
 import frc.robot.io.PieceVisionIO;
@@ -32,6 +33,16 @@ public void periodic() {
 	public void setPriorityId(int id) {
 		priorityIdEntry.setNumber(id);
 	}
+
+	public double findDistance() {
+		double targetOffsetAngle_Vertical = fieldInputs.ty;
+		double angleToGoalDegrees = VisionConstants.limelightMountAngleDegrees + targetOffsetAngle_Vertical;
+		double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
+
+		double distance = (VisionConstants.goalHeight - VisionConstants.LensHeight) / Math.tan(angleToGoalRadians);
+
+		return distance;
+	}
 }
 
 /*