From 05807a663ed9b36a95f60537c24a56be312145ce Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 3 Apr 2026 17:48:56 -0400 Subject: [PATCH] Turret Auto aligns with chassis --- .../commands/AutoShootCommandChassisTurret.java | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 3aa0432..629bfe5 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -117,7 +117,18 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]); // Calculate turret Angle - double targetDistance = targetsPose[2]; + double cameraDistanceToTarget = targetsPose[2]; + double cameraToShootDistance = 0.26035; + // a^2 + b^2 = c^2 + double shooterDistanceToTarget = Math.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2)); + double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget); + Logger.recordOutput("AutoAlign-TurretAngleOfTriagle", angleAtShooter); + double robotTurretAngle = (Math.PI/2) - angleAtShooter; + Logger.recordOutput("AutoAlign-TurretAngleRelativeToRobot", robotTurretAngle); + double robotTurretRotations = (robotTurretAngle / Math.PI) * Constants.ShooterConstants.TURRET_GEAR_RATIO; + Logger.recordOutput("AutoAlign-TurretRotations", robotTurretRotations); + + turretSubsystem.setTurretMotorRotation(-robotTurretRotations); driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated }