From 96af7ce35b5b635142541b99d45494ea692fc70e Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Thu, 19 Feb 2026 02:41:50 -0500 Subject: [PATCH 1/4] implemented autoaim turret code into subsystem --- src/main/java/frc/robot/RobotContainer.java | 6 ++ .../frc/robot/commands/turret/AimTurret.java | 89 +++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 7 ++ .../robot/utils/math/TurretCalculations.java | 41 +++++++++ 4 files changed, 143 insertions(+) create mode 100644 src/main/java/frc/robot/commands/turret/AimTurret.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b9ac010..746cba8e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.commands.angler.AimAngler; import frc.robot.commands.angler.RunAnglerToReverseLimit; import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.turret.AimTurret; import frc.robot.commands.turret.RunTurretToFwdLimit; import frc.robot.commands.turret.RunTurretToRevLimit; import frc.robot.commands.turret.SetTurretAngle; @@ -63,6 +64,7 @@ import frc.robot.apriltags.TCPApriltagIo; import java.io.File; +import java.util.function.Supplier; /** * This class is where the bulk of the robot should be declared. Since @@ -88,6 +90,7 @@ public class RobotContainer { private final HopperSubsystem hopperSubsystem; private final TurretSubsystem turretSubsystem; private final IntakeDeployerSubsystem intakeDeployer; + private final Supplier poseSupplier; private SwerveSubsystem drivebase = null; private GyroSubsystem gyroSubsystem = null; private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT); @@ -384,6 +387,9 @@ public void putShuffleboardCommands() { SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem, new ApriltagReading(0, 0, 0, 0, 0, 0, 0))); + SmartDashboard.putData("Aim Turret", + new AimTurret(turretSubsystem, poseSupplier, getShootingState())); + } // basic drive command diff --git a/src/main/java/frc/robot/commands/turret/AimTurret.java b/src/main/java/frc/robot/commands/turret/AimTurret.java new file mode 100644 index 00000000..c57b4ea3 --- /dev/null +++ b/src/main/java/frc/robot/commands/turret/AimTurret.java @@ -0,0 +1,89 @@ +package frc.robot.commands.turret; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.constants.GameConstants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; +import frc.robot.utils.math.TurretCalculations; +import java.util.function.Supplier; + +/** + * Default command that aims the turret based on robot pose and shooting state. + * + */ +public class AimTurret extends LoggableCommand { + + private final TurretSubsystem turret; + private final Supplier poseSupplier; + private final ShootingState shootingState; + private final boolean isBlueAlliance; + + public AimTurret(TurretSubsystem turret, Supplier poseSupplier, ShootingState shootingState) { + this.turret = turret; + this.poseSupplier = poseSupplier; + this.shootingState = shootingState; + this.isBlueAlliance = isBlue(); + addRequirements(turret); + } + + @Override + public void execute() { + Pose2d robotPose = poseSupplier.get(); + ShootState state = shootingState.getShootState(); + handleState(state, robotPose, isBlueAlliance); + } + + @Override + public boolean isFinished() { + return false; + } + + private void handleState(ShootState state, Pose2d robotPose, boolean isBlueAlliance) { + switch (state) { + case STOPPED: turret.stopMotors(); + // set to fixed turret angle at the various fixed states + case FIXED: new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_1); + case FIXED_2: new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_2); + // auto aim to hub + case SHOOTING_HUB: AutoShoot(turret, robotPose, isBlueAlliance); + // auto aim to shuttle site + case SHUTTLING: ShuttleShoot(turret, robotPose, isBlueAlliance); + } + } + + /** Moves turret to position automatically calculated based on distance from the hub. + * + * @param turret + * @param robotPose + * @param isBlueAlliance + */ + private void AutoShoot(TurretSubsystem turret, Pose2d robotPose, boolean isBlueAlliance) { + double targetAngle = TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), isBlueAlliance); + new SetTurretAngle(turret, targetAngle); + } + + /** Moves turret to shuttling position based on distance from shuttling position + * + * @param turret + * @param robotPose + * @param isBlueAlliance + */ + private void ShuttleShoot(TurretSubsystem turret, Pose2d robotPose, boolean isBlueAlliance) { + double targetAngle = TurretCalculations.calculateTurretShuttleAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), isBlueAlliance); + new SetTurretAngle(turret, targetAngle); + } + + /** + * Checks if the alliance is blue, defaults to false if alliance isn't available. + * + * @return true if the blue alliance, false if red. Defaults to false if none is available. + */ + private boolean isBlue() { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Blue : false; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index a350e28c..928da131 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -57,6 +57,9 @@ public enum Mode { public static final double INITIAL_INTAKE_DEPLOYER_SPEED = 10; public static final double INITIAL_INTAKE_RETRACTION_SPEED = -10; + // Fixed Angles + public static final double FIXED_TURRET_ANGLE_1 = 0; + public static final double FIXED_TURRET_ANGLE_2 = 0; //Timeouts public static final double SPIN_TIMEOUT = 5; @@ -135,6 +138,10 @@ public enum Mode { public static final double BLUE_HUB_Y_POSITION = 4.0345; public static final double RED_HUB_X_POSITION = 11.9154; public static final double RED_HUB_Y_POSITION = 4.0345; + public static final double BLUE_SHUTTLE_X_POSITION = 3; + public static final double BLUE_SHUTTLE_Y_POSITION = 4.0345; + public static final double RED_SHUTTLE_X_POSITION = 3; + public static final double RED_SHUTTLE_Y_POSITION = 4.0345; public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index d2aea690..efee4832 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -68,4 +68,45 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do } + // shuttlePosX and shuttlePosY are given values from the constants file -- gives the x and y positions of the shuttle site (in meters) + public static double calculateTurretShuttleAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) { + + // calculates the position of the turret with respect to the origin using the robot center + // and the constant distance between the robot center and the turret. + double turretPosX = robotPosX + GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET; + double turretPosY = robotPosY + GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET; + + double shuttlePosX; + double shuttlePosY; + + if (isBlueAlliance) { + // shuttle position determined by which alliance robot is on + shuttlePosX = GameConstants.BLUE_SHUTTLE_X_POSITION; + shuttlePosY = GameConstants.BLUE_SHUTTLE_Y_POSITION; + } else { + shuttlePosX = GameConstants.RED_SHUTTLE_X_POSITION; + shuttlePosY = GameConstants.RED_SHUTTLE_Y_POSITION; + } + + /* + * This finds the unadjusted pan angle (assuming there is no robot rotation) using + * trigonometry. We take the arctangent of the y-distance beween the robot and the shuttle position + * and the x-distance between the robot and the shuttle position, giving us the unadjusted pan + * angle. The function atan2 ensures the sign of the angle is correct based on the signs + * of the input numbers. + * + */ + double panAngleUnadjusted = Math.atan2(shuttlePosY - turretPosY, shuttlePosX - turretPosX); + + /* + * Adjusts the pan angle to account for the robot's current rotation. We subtract the + * angle of the robot's rotation from the unadjusted angle of the turret to find the + * pan angle, which is the proper angle of the turret adjusted for the robot's rotation. + */ + double panAngle = panAngleUnadjusted - robotRotation; + + return panAngle; + + } + } From c526bf55258d9a3f5cf811cad0eb88aada08f01c Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Thu, 19 Feb 2026 02:43:14 -0500 Subject: [PATCH 2/4] fixed --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 746cba8e..1c2ef83a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,7 +90,7 @@ public class RobotContainer { private final HopperSubsystem hopperSubsystem; private final TurretSubsystem turretSubsystem; private final IntakeDeployerSubsystem intakeDeployer; - private final Supplier poseSupplier; + private final Supplier poseSupplier = null; private SwerveSubsystem drivebase = null; private GyroSubsystem gyroSubsystem = null; private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT); From b5d1ca9252d2a8ddcb1e6759502af121586607d0 Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Thu, 19 Feb 2026 12:36:21 -0500 Subject: [PATCH 3/4] fixed comment --- src/main/java/frc/robot/commands/turret/AimTurret.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/turret/AimTurret.java b/src/main/java/frc/robot/commands/turret/AimTurret.java index c57b4ea3..d27cf7ab 100644 --- a/src/main/java/frc/robot/commands/turret/AimTurret.java +++ b/src/main/java/frc/robot/commands/turret/AimTurret.java @@ -54,7 +54,7 @@ private void handleState(ShootState state, Pose2d robotPose, boolean isBlueAllia } } - /** Moves turret to position automatically calculated based on distance from the hub. + /** Moves turret to position automatically calculated based on angle from the hub. * * @param turret * @param robotPose @@ -65,7 +65,7 @@ private void AutoShoot(TurretSubsystem turret, Pose2d robotPose, boolean isBlueA new SetTurretAngle(turret, targetAngle); } - /** Moves turret to shuttling position based on distance from shuttling position + /** Moves turret to shuttling position based on angle from shuttling position * * @param turret * @param robotPose From ff02d6b772fed76415385ff7c9cd13e3b012cebb Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Thu, 19 Feb 2026 17:56:40 -0500 Subject: [PATCH 4/4] created default command and fixed poseSupplier --- src/main/java/frc/robot/RobotContainer.java | 6 +-- .../frc/robot/commands/turret/AimTurret.java | 54 +++++++++++-------- 2 files changed, 34 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1c2ef83a..d35a1277 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,7 +90,6 @@ public class RobotContainer { private final HopperSubsystem hopperSubsystem; private final TurretSubsystem turretSubsystem; private final IntakeDeployerSubsystem intakeDeployer; - private final Supplier poseSupplier = null; private SwerveSubsystem drivebase = null; private GyroSubsystem gyroSubsystem = null; private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT); @@ -227,6 +226,8 @@ private void configureBindings() { Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); } + + turretSubsystem.setDefaultCommand(new AimTurret(turretSubsystem, drivebase::getPose, getShootingState())); } public void putShuffleboardCommands() { @@ -387,9 +388,6 @@ public void putShuffleboardCommands() { SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem, new ApriltagReading(0, 0, 0, 0, 0, 0, 0))); - SmartDashboard.putData("Aim Turret", - new AimTurret(turretSubsystem, poseSupplier, getShootingState())); - } // basic drive command diff --git a/src/main/java/frc/robot/commands/turret/AimTurret.java b/src/main/java/frc/robot/commands/turret/AimTurret.java index d27cf7ab..8653dcc9 100644 --- a/src/main/java/frc/robot/commands/turret/AimTurret.java +++ b/src/main/java/frc/robot/commands/turret/AimTurret.java @@ -33,7 +33,7 @@ public AimTurret(TurretSubsystem turret, Supplier poseSupplier, Shooting public void execute() { Pose2d robotPose = poseSupplier.get(); ShootState state = shootingState.getShootState(); - handleState(state, robotPose, isBlueAlliance); + handleState(state, robotPose); } @Override @@ -41,45 +41,55 @@ public boolean isFinished() { return false; } - private void handleState(ShootState state, Pose2d robotPose, boolean isBlueAlliance) { + private void handleState(ShootState state, Pose2d robotPose) { switch (state) { - case STOPPED: turret.stopMotors(); - // set to fixed turret angle at the various fixed states - case FIXED: new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_1); - case FIXED_2: new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_2); - // auto aim to hub - case SHOOTING_HUB: AutoShoot(turret, robotPose, isBlueAlliance); - // auto aim to shuttle site - case SHUTTLING: ShuttleShoot(turret, robotPose, isBlueAlliance); + case STOPPED: + turret.stopMotors(); + // set to fixed turret angle at the various fixed states + case FIXED: + new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_1); + case FIXED_2: + new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_2); + // auto aim to hub + case SHOOTING_HUB: + AutoShoot(turret, robotPose); + // auto aim to shuttle site + case SHUTTLING: + ShuttleShoot(turret, robotPose); } } - /** Moves turret to position automatically calculated based on angle from the hub. + /** + * Moves turret to position automatically calculated based on angle from the + * hub. * * @param turret * @param robotPose - * @param isBlueAlliance */ - private void AutoShoot(TurretSubsystem turret, Pose2d robotPose, boolean isBlueAlliance) { - double targetAngle = TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), isBlueAlliance); + private void AutoShoot(TurretSubsystem turret, Pose2d robotPose) { + double targetAngle = TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), isBlueAlliance); new SetTurretAngle(turret, targetAngle); } - /** Moves turret to shuttling position based on angle from shuttling position + /** + * Moves turret to shuttling position based on angle from shuttling position * * @param turret * @param robotPose - * @param isBlueAlliance - */ - private void ShuttleShoot(TurretSubsystem turret, Pose2d robotPose, boolean isBlueAlliance) { - double targetAngle = TurretCalculations.calculateTurretShuttleAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), isBlueAlliance); + */ + private void ShuttleShoot(TurretSubsystem turret, Pose2d robotPose) { + double targetAngle = TurretCalculations.calculateTurretShuttleAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), isBlueAlliance); new SetTurretAngle(turret, targetAngle); } - /** - * Checks if the alliance is blue, defaults to false if alliance isn't available. + /** + * Checks if the alliance is blue, defaults to false if alliance isn't + * available. * - * @return true if the blue alliance, false if red. Defaults to false if none is available. + * @return true if the blue alliance, false if red. Defaults to false if none is + * available. */ private boolean isBlue() { var alliance = DriverStation.getAlliance();