From 96af7ce35b5b635142541b99d45494ea692fc70e Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Thu, 19 Feb 2026 02:41:50 -0500 Subject: [PATCH 01/10] 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 02/10] 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 03/10] 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 04/10] 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(); From 0540eb0bbdea779bb2d5e356ad7e70ab8f085086 Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Sat, 21 Feb 2026 16:16:06 -0500 Subject: [PATCH 05/10] applied turret aim code to controller subsystem --- .../frc/robot/commands/turret/AimTurret.java | 99 ------------------- .../commands/turret/DefaultTurretControl.java | 2 +- .../robot/subsystems/ControllerSubsystem.java | 37 ++++++- 3 files changed, 33 insertions(+), 105 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/turret/AimTurret.java diff --git a/src/main/java/frc/robot/commands/turret/AimTurret.java b/src/main/java/frc/robot/commands/turret/AimTurret.java deleted file mode 100644 index 8653dcc9..00000000 --- a/src/main/java/frc/robot/commands/turret/AimTurret.java +++ /dev/null @@ -1,99 +0,0 @@ -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); - } - - @Override - public boolean isFinished() { - return false; - } - - 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); - // auto aim to shuttle site - case SHUTTLING: - ShuttleShoot(turret, robotPose); - } - } - - /** - * Moves turret to position automatically calculated based on angle from the - * hub. - * - * @param turret - * @param robotPose - */ - 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 - * - * @param turret - * @param robotPose - */ - 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. - * - * @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/commands/turret/DefaultTurretControl.java b/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java index 1fb25cdb..ced4fb44 100644 --- a/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java +++ b/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java @@ -17,7 +17,7 @@ public DefaultTurretControl(TurretSubsystem turretSubsystem, ControllerSubsystem @Override public void execute() { - turretSubsystem.setAngle(controllerSubsystem.getTargetTurretAngleDegrees()); + new SetTurretAngle(turretSubsystem, controllerSubsystem.getTargetTurretAngleDegrees()); } @Override diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 6f2b0a28..3e86987e 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -5,6 +5,7 @@ import frc.robot.RobotContainer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -12,6 +13,7 @@ import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.math.TurretCalculations; public class ControllerSubsystem extends SubsystemBase { @@ -36,9 +38,9 @@ public class ControllerSubsystem extends SubsystemBase { private static final ShotTargets STOPPED_TARGETS = new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, false); private static final ShotTargets FIXED_TARGETS = - new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true); + new ShotTargets(10.0, 120.0, 15.0, 0.0, true, true); private static final ShotTargets FIXED_2_TARGETS = - new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); + new ShotTargets(22.0, 180.0, -15.0, 0.0, true, true); // Placeholder pose-driven profiles. private static final PoseControlProfile HUB_PROFILE = @@ -179,11 +181,24 @@ private double calculateShooterVelocity(double computedDistanceMeters, PoseContr return profile.defaultShooterVelocityRpm; } + // calculates turret angle from calculations private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { - // TODO: Replace with distance turret angle calculation. - return profile.defaultTurretAngleDegrees; - } + + double targetAngle = 0; + + if (getCurrentShootState() == ShootState.SHOOTING_HUB) { + + targetAngle = TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), isBlue()); + } else if (getCurrentShootState() == ShootState.SHUTTLING) { + + targetAngle = TurretCalculations.calculateTurretShuttleAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), isBlue()); + } + return targetAngle; + } + //Getters for all the subsystems to set posistion. public double getTargetAnglerAngleDegrees() { return activeTargets.anglerAngleDegrees; @@ -261,4 +276,16 @@ private PoseControlProfile( this.defaultTurretAngleDegrees = defaultTurretAngleDegrees; } } + + /** + * 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; + } } From 8f5ec71adf196d2fbc5633e045227d78a3589256 Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Mon, 2 Mar 2026 18:41:53 -0500 Subject: [PATCH 06/10] fixed error --- src/main/java/frc/robot/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5cc42d46..39b13d87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,7 +39,6 @@ import frc.robot.commands.shooter.DefaultShooterControl; import frc.robot.commands.auto.ExampleAuto; import frc.robot.commands.shooter.SetShootingState; -import frc.robot.commands.turret.AimTurret; import frc.robot.commands.testing.RunDashboardShotTest; import frc.robot.commands.turret.DefaultTurretControl; import frc.robot.commands.turret.RunTurretToFwdLimit; @@ -334,7 +333,6 @@ private void configureBindings() { drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); } - turretSubsystem.setDefaultCommand(new AimTurret(turretSubsystem, drivebase::getPose, getShootingState())); } public void putShuffleboardCommands() { From 23f25f2349c09001477d5a85b983c30a48db2c15 Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Mon, 2 Mar 2026 19:09:25 -0500 Subject: [PATCH 07/10] created bounds on turret --- .../robot/commands/turret/SetTurretAngle.java | 43 +++++++++++-------- .../frc/robot/constants/GameConstants.java | 2 - 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/commands/turret/SetTurretAngle.java b/src/main/java/frc/robot/commands/turret/SetTurretAngle.java index 8b76311d..171ae7f9 100644 --- a/src/main/java/frc/robot/commands/turret/SetTurretAngle.java +++ b/src/main/java/frc/robot/commands/turret/SetTurretAngle.java @@ -1,5 +1,7 @@ package frc.robot.commands.turret; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.constants.GameConstants; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; @@ -7,24 +9,31 @@ * Runs the turret to the target angle */ public class SetTurretAngle extends LoggableCommand { - private final TurretSubsystem turret; - private double targetAngle; - - + private final TurretSubsystem turret; + private double targetAngle; + private boolean turretInRange; + + public SetTurretAngle(TurretSubsystem turret, double targetAngle) { + this.turret = turret; + this.targetAngle = targetAngle; + this.turretInRange = false; + addRequirements(turret); + } - public SetTurretAngle(TurretSubsystem turret, double targetAngle) { - this.turret = turret; - this.targetAngle = targetAngle; - addRequirements(turret); - } - - @Override - public void initialize() { - } - - @Override - public void execute() { - turret.setAngle(targetAngle); + @Override + public void initialize() { + if ((targetAngle > GameConstants.TURRET_LEFT_ANGLE + 5) && + (targetAngle < GameConstants.TURRET_RIGHT_ANGLE - 5)) { + turretInRange = true; + } + } + + @Override + public void execute() { + SmartDashboard.putBoolean("WITHIN_SHOOTING_RANGE", turretInRange); + if (turretInRange) { + turret.setAngle(targetAngle); + } } @Override diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index f1665440..07dfd82b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -154,8 +154,6 @@ public enum Mode { 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 // Shift timings public static final int SHIFT_1_START = 130; From e49dfc2cf5d1d5b6d33795af223642e96767af65 Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Mon, 2 Mar 2026 19:21:35 -0500 Subject: [PATCH 08/10] fixed error --- .../frc/robot/utils/math/TurretCalculations.java | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index efee4832..9f1ca69f 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -30,11 +30,6 @@ public class TurretCalculations { // robotRotation is the angle between the horizontal (by the alliance side chute) and the robot public static double calculateTurretAngle(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 hubPosX; double hubPosY; @@ -55,7 +50,7 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do * of the input numbers. * */ - double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX); + double panAngleUnadjusted = Math.atan2(hubPosY - robotPosY, hubPosX - robotPosX); /* * Adjusts the pan angle to account for the robot's current rotation. We subtract the @@ -70,11 +65,6 @@ 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; @@ -96,7 +86,7 @@ public static double calculateTurretShuttleAngle(double robotPosX, double robotP * of the input numbers. * */ - double panAngleUnadjusted = Math.atan2(shuttlePosY - turretPosY, shuttlePosX - turretPosX); + double panAngleUnadjusted = Math.atan2(shuttlePosY - robotPosY, shuttlePosX - robotPosX); /* * Adjusts the pan angle to account for the robot's current rotation. We subtract the From 09a61e120dd799adecfa392898edff7cf280ac6f Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Mon, 2 Mar 2026 19:24:01 -0500 Subject: [PATCH 09/10] fixed more errors --- .../java/frc/robot/utils/math/LaunchCalculations.java | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/utils/math/LaunchCalculations.java b/src/main/java/frc/robot/utils/math/LaunchCalculations.java index 5ada137a..c4e1ec03 100644 --- a/src/main/java/frc/robot/utils/math/LaunchCalculations.java +++ b/src/main/java/frc/robot/utils/math/LaunchCalculations.java @@ -22,11 +22,6 @@ public static double calculateShooterVelocity(double robotPosX, double robotPosY // Target value -- what we're trying to find double shooterVelocity; // Initial velocity of the shooter -- related to flywheel speed - // 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; - // Given values -- from Constants file double hubHeight = GameConstants.HUB_HEIGHT; // height of the top of the hub double shooterHeight = GameConstants.SHOOTER_HEIGHT; // height of shooter @@ -47,8 +42,8 @@ public static double calculateShooterVelocity(double robotPosX, double robotPosY } // Compute distance between hub and robot with Pythagorean Theorem - deltaDistance = Math.sqrt(Math.pow(hubPosY - turretPosY, 2) - + Math.pow(hubPosX - turretPosX, 2)); + deltaDistance = Math.sqrt(Math.pow(hubPosY - robotPosY, 2) + + Math.pow(hubPosX - robotPosX, 2)); // Compute the change in height between the shooter and the hub deltaHeight = hubHeight - shooterHeight; From 26bd2a925661318a0e8b13fc41d568bbed5948e6 Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Tue, 3 Mar 2026 18:27:38 -0500 Subject: [PATCH 10/10] removed unit tests --- .../calculations/LaunchCalculationsTest.java | 7 --- .../calculations/TurretCalculationsTest.java | 53 ------------------- 2 files changed, 60 deletions(-) delete mode 100644 src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java delete mode 100644 src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java diff --git a/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java b/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java deleted file mode 100644 index e6692667..00000000 --- a/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.utils.calculations; - -public class LaunchCalculationsTest { - public LaunchCalculationsTest() { - - } -} diff --git a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java b/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java deleted file mode 100644 index 276eab71..00000000 --- a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.robot.utils.calculations; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import frc.robot.utils.math.TurretCalculations; - -public class TurretCalculationsTest { - - static final double DELTA = .0348; // standard deviation of 2 degrees or .0348 radians - double[] xPos = {5.827, 13.850, 9.211, 1.211, 3.811}; - double[] yPos = {2.359, 7.921, 5.386, 0.857, 4.035}; - double[] robotRotation = {toRadians(30), toRadians(-53), toRadians(180), toRadians(45), toRadians(-45)}; - double[] panAngles = {toRadians(-18), toRadians(-102), toRadians(-217), toRadians(-2), toRadians(-1)}; - boolean[] isBlueAlliance = {false, true, false, true, true}; - - private int index; - - private double toRadians(double degree) { - return degree * Math.PI / 180; - } - - @Test - void index0Test() { - index = 0; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index1Test() { - index = 1; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index2Test() { - index = 2; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index3Test() { - index = 3; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index4Test() { - index = 4; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } -} \ No newline at end of file