From 61e585826968bcadefc28f2d6cc43abdc4bc2e4d Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 10 Mar 2026 09:26:15 -0700 Subject: [PATCH 1/6] add fixed shots theoretically --- src/main/java/frc/robot/Superstructure.java | 21 ++++- .../subsystems/shooter/TurretSubsystem.java | 76 ++++++++++++++----- .../java/frc/robot/utils/autoaim/AutoAim.java | 12 +++ 3 files changed, 91 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 206aadac..741eb874 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -74,6 +74,13 @@ public enum FeedTarget { RIGHT } + public enum FixedShotTarget { + LEFT, + MID, + RIGHT, + NONE + } + @AutoLogOutput(key = "Superstructure/State") private static SuperState state = SuperState.IDLE; @@ -138,9 +145,12 @@ public enum FeedTarget { @AutoLogOutput(key = "Superstructure/Ready?") private Trigger readyTrigger; - @AutoLogOutput(key = "Superstructure/Operator Override?") + @AutoLogOutput(key = "Superstructure/Operator Pose Override?") private boolean override; + @AutoLogOutput(key = "Superstructure/Fixed Shot") + private static FixedShotTarget fixedShotTarget = FixedShotTarget.NONE; + /** Creates a new Superstructure. */ public Superstructure( SwerveSubsystem swerve, @@ -188,6 +198,11 @@ private void addTriggers() { operator.leftTrigger().onTrue(Commands.runOnce(() -> override = true)); operator.rightTrigger().onTrue(Commands.runOnce(() -> override = false)); + operator.povLeft().onTrue(Commands.runOnce(() -> fixedShotTarget = FixedShotTarget.LEFT)); + operator.povUp().onTrue(Commands.runOnce(() -> fixedShotTarget = FixedShotTarget.MID)); + operator.povRight().onTrue(Commands.runOnce(() -> fixedShotTarget = FixedShotTarget.RIGHT)); + operator.povDown().onTrue(Commands.runOnce(() -> fixedShotTarget = FixedShotTarget.NONE)); + shootReq = driver .rightTrigger() @@ -680,4 +695,8 @@ public static ShotTarget getShotTarget() { public static FeedTarget getFeedTarget() { return feedTarget; } + + public static FixedShotTarget getFixedShotTarget() { + return fixedShotTarget; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index ceb8dffd..77e4ed70 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.Superstructure; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; @@ -458,15 +459,35 @@ public Command score( Supplier chassisSpeedsSupplier) { return this.run( () -> { - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + switch (Superstructure.getFixedShotTarget()) { + // in front of left trench with intake facing trench + case LEFT: + hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(Rotation2d.kZero); // TODO + // in front of tower with intake facing left (to avoid deadzone) + case MID: + hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(Rotation2d.kZero); // TODO + // in front of right trench with intake facing alliance wall + case RIGHT: + hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(Rotation2d.kZero); // TODO + case NONE: + hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + shotDataSupplier.get().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); + } }); } @@ -639,14 +660,35 @@ public Command spinUp( Supplier chassisSpeedsSupplier) { return this.run( () -> { - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + switch (Superstructure.getFixedShotTarget()) { + // in front of left trench with intake facing trench + case LEFT: + hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(Rotation2d.kZero); // TODO + // in front of tower with intake facing left (to avoid deadzone) + case MID: + hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(Rotation2d.kZero); // TODO + // in front of right trench with intake facing alliance wall + case RIGHT: + hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(Rotation2d.kZero); // TODO + case NONE: + hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + shotDataSupplier.get().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); + } }); } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index e911a3f9..cd04d464 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -255,4 +255,16 @@ public static ShotData getCompensatedSOTMShotData( public static boolean targetInTurretDeadzone() { return outOfRange; } + + public static ShotData getLeftFixedShotData() { + return new ShotData(Rotation2d.kZero, 0, 0); + } + + public static ShotData getRightFixedShotData() { + return new ShotData(Rotation2d.kZero, 0, 0); + } + + public static ShotData getMidFixedShotData() { + return new ShotData(Rotation2d.kZero, 0, 0); + } } From 02acd150266037029aad50969f3cb44f85e6546f Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 10 Mar 2026 12:57:28 -0700 Subject: [PATCH 2/6] add preliminary turret zeroing --- src/main/java/frc/robot/Superstructure.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 4 ++++ .../subsystems/shooter/TurretSubsystem.java | 22 ++++++++++++++++--- 3 files changed, 24 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 741eb874..b397038f 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -322,7 +322,7 @@ private void addTransitions() { (preClimbReq.and(climbReq.negate())).onTrue(changeStateTo(SuperState.PRE_CLIMB)); - bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, climbReq); + bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, climbReq); //TODO maybe add transition out of climb in case we fall bindTransition( SuperState.PRE_CLIMB, SuperState.IDLE, preClimbReq.negate().and(climbReq.negate())); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a5344d3f..75651107 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -113,4 +113,8 @@ public default void turretInit() {} public default Pose2d getTurretPose(Pose2d robotPose) { return Pose2d.kZero; } + + public default Command currentZeroTurret() { + return Commands.none(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 77e4ed70..2d4489e9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -51,6 +51,7 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final Rotation2d HOOD_MAX_ANGLE = Rotation2d.fromDegrees(73); public static final Rotation2d HOOD_MIN_ANGLE = Rotation2d.fromDegrees(23.16); public static final double HOOD_CURRENT_ZERO_THRESHOLD = 30.0; + public static final double TURRET_CURRENT_ZERO_THRESHOLD = 30.0; // TODO find public static final double FLYWHEEL_DIAMETER_INCHES = 4; @@ -73,7 +74,8 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final Translation2d ROBOT_TO_TURRET_TRANSLATION = new Translation2d(-0.177413, -0.111702); // , 0.350341); public static final double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; - double currentFilterValue = 0.0; + double hoodCurrentFilterValue = 0.0; + double turretCurrentFilterValue = 0.0; private CANcoderIO cancoder24t; private CANcoderIO cancoder26t; @@ -183,7 +185,7 @@ public void periodic() { Logger.processInputs("Shooter/Turret Cancoder24t", cancoder24tInputs); cancoder26t.updateInputs(cancoder26tInputs); Logger.processInputs("Shooter/Turret Cancoder26t", cancoder26tInputs); - currentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); + hoodCurrentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); cancoder24tDisconnectedAlert.set(!cancoder24tInputs.connected); cancoder26tDisconnectedAlert.set(!cancoder26tInputs.connected); @@ -399,7 +401,7 @@ public Command zeroHood() { public Command runCurrentZeroing() { return this.run(() -> hoodIO.setHoodVoltage(-3.0)) .until( - new Trigger(() -> Math.abs(currentFilterValue) > HOOD_CURRENT_ZERO_THRESHOLD) + new Trigger(() -> Math.abs(hoodCurrentFilterValue) > HOOD_CURRENT_ZERO_THRESHOLD) .debounce(0.25)) .andThen(Commands.parallel(Commands.print("Hood Zeroed"), zeroHood())); } @@ -696,4 +698,18 @@ public Command spinUp( public Pose2d getTurretPose(Pose2d robotPose) { return robotPose.transformBy(new Transform2d(ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); } + + @Override + public Command currentZeroTurret() { + // TODO idk which hardstop we should zero against but + return this.run(() -> turretIO.setVoltage(1.0)) + .until( + new Trigger(() -> Math.abs(turretCurrentFilterValue) > TURRET_CURRENT_ZERO_THRESHOLD) + .debounce(0.25)) + .andThen(Commands.parallel(Commands.print("Turret Zeroed"), zeroTurret())); + } + + public Command zeroTurret() { + return this.runOnce(() -> turretIO.resetTurretEncoder(TURRET_FORWARD_HARDSTOP_ANGLE)); + } } From 2a1f740bae22bd14de5e56d12c872d3ce758422f Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 10 Mar 2026 12:59:18 -0700 Subject: [PATCH 3/6] made constants --- .../frc/robot/subsystems/shooter/TurretSubsystem.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 2d4489e9..03b8b667 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -55,6 +55,11 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final double FLYWHEEL_DIAMETER_INCHES = 4; + public static final Rotation2d TURRET_LEFT_FIXED_SHOT_ANGLE = Rotation2d.kZero; + public static final Rotation2d TURRET_RIGHT_FIXED_SHOT_ANGLE = Rotation2d.kZero; + public static final Rotation2d TURRET_MID_FIXED_SHOT_ANGLE = Rotation2d.kZero; + + // TODO: REDO THIS HARDSTOP WHEN FIXED?? // logged for ease of graph viewing @AutoLogOutput(key = "Shooter/Turret/Rear Hardstop") @@ -668,19 +673,19 @@ public Command spinUp( hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(Rotation2d.kZero); // TODO + turretIO.setTurretPosition(TURRET_LEFT_FIXED_SHOT_ANGLE); // TODO // in front of tower with intake facing left (to avoid deadzone) case MID: hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(Rotation2d.kZero); // TODO + turretIO.setTurretPosition(TURRET_MID_FIXED_SHOT_ANGLE); // TODO // in front of right trench with intake facing alliance wall case RIGHT: hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(Rotation2d.kZero); // TODO + turretIO.setTurretPosition(TURRET_RIGHT_FIXED_SHOT_ANGLE); // TODO case NONE: hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( From 0ada0b2885f910700b61001a66155b20917def9e Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 10 Mar 2026 16:32:25 -0700 Subject: [PATCH 4/6] lock hood under trench in tele --- src/main/java/frc/robot/Superstructure.java | 18 +++++++++++++----- .../subsystems/swerve/SwerveSubsystem.java | 14 ++++++++++++++ 2 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 864bd733..4519572f 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -110,7 +110,7 @@ public enum FeedTarget { @AutoLogOutput(key = "Superstructure/Score Request") private Trigger scoreReq = new Trigger(() -> shotTarget == ShotTarget.SCORE) - .and(() -> canScore()) + // .and(() -> canScore()) .or(Autos.autoScoreReq); @AutoLogOutput(key = "Superstructure/Feed Request") @@ -185,7 +185,9 @@ private void addTriggers() { driver .rightTrigger() .and(DriverStation::isTeleop) - .or(Autos.autoScoreReq); // Maybe should include if its our turn? //TODO fix auto + .and(() -> canShoot()) + .or(Autos.autoScoreReq) + .and(() -> canShoot()); // Maybe should include if its our turn? //TODO fix auto // bindings intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); @@ -231,7 +233,7 @@ private void addTransitions() { bindTransition(SuperState.SPIN_UP_SCORE, SuperState.IDLE, shootReq.negate()); - bindTransition(SuperState.SCORE, SuperState.IDLE, shootReq.negate().or(scoreReq.negate())); + bindTransition(SuperState.SCORE, SuperState.IDLE, shootReq.negate()); // SCORE_FLOW transitions { @@ -245,14 +247,16 @@ private void addTransitions() { bindTransition( SuperState.SPIN_UP_SCORE_FLOW, SuperState.IDLE, - intakeReq.negate().and(shootReq.negate()).or(scoreReq.negate())); + intakeReq.negate().and(shootReq.negate())); bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate()); bindTransition( - SuperState.SCORE_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()).or(scoreReq.negate())); + SuperState.SCORE_FLOW, + SuperState.IDLE, + intakeReq.negate().and(shootReq.negate())); } // -------------------------------------------------------------------------- @@ -664,6 +668,10 @@ public boolean canScore() { return (isOurShift() || !DriverStation.isFMSAttached()) && (inScoringArea() || override); } + public boolean canShoot() { + return !swerve.isNearTrenchForHood(); + } + public static ShotTarget getShotTarget() { return shotTarget; } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index b3aba139..9f40ba86 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -762,6 +762,20 @@ public boolean isCloseToTrench() { return inXTol && inYTol; } + @AutoLogOutput(key = "Swerve/Near Trench for hood") + public boolean isNearTrenchForHood() { + double x = getPose().getX(); + double y = getPose().getY(); + + boolean inXTol = + MathUtil.isNear(TrenchPoses.BLUE_RIGHT.getPose().getX(), x, 1) + || MathUtil.isNear(TrenchPoses.RED_RIGHT.getPose().getX(), x, 1); + boolean inYTol = + MathUtil.isNear(TrenchPoses.BLUE_RIGHT.getPose().getY(), y, 0.515) + || MathUtil.isNear(TrenchPoses.RED_RIGHT.getPose().getY(), y, 0.515); + return inXTol && inYTol; + } + public boolean isCloseToBump() { double x = getPose().getX(); double y = getPose().getY(); From 78bf7b157e1e096eac7ed66e1281a99061bd6d93 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 11 Mar 2026 00:10:26 -0700 Subject: [PATCH 5/6] populate fixed shot numbers --- src/main/java/frc/robot/Superstructure.java | 9 +++++---- .../java/frc/robot/subsystems/shooter/FlywheelIO.java | 2 +- .../java/frc/robot/subsystems/shooter/Shooter.java | 6 +++--- .../frc/robot/subsystems/shooter/TurretSubsystem.java | 9 ++++----- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 10 +++++++--- 5 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 221377f5..33e99841 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -276,9 +276,7 @@ private void addTransitions() { bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate()); bindTransition( - SuperState.SCORE_FLOW, - SuperState.IDLE, - intakeReq.negate().and(shootReq.negate())); + SuperState.SCORE_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate())); } // -------------------------------------------------------------------------- @@ -326,7 +324,10 @@ private void addTransitions() { (preClimbReq.and(climbReq.negate())).onTrue(changeStateTo(SuperState.PRE_CLIMB)); - bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, climbReq); //TODO maybe add transition out of climb in case we fall + bindTransition( + SuperState.PRE_CLIMB, + SuperState.CLIMB, + climbReq); // TODO maybe add transition out of climb in case we fall bindTransition( SuperState.PRE_CLIMB, SuperState.IDLE, preClimbReq.negate().and(climbReq.negate())); } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index c8fd9577..632c5f84 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -196,7 +196,7 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelFollowerTorqueCurrent = flywheelFollowerTorqueCurrent.getValueAsDouble(); } - @AutoLogOutput(key = "Shooter/Setpoint") + @AutoLogOutput(key = "Shooter/Flywheel/Setpoint") public double getSetpointRotPerSec() { return velocitySetpointRotPerSec; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 75651107..000e7d57 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -114,7 +114,7 @@ public default Pose2d getTurretPose(Pose2d robotPose) { return Pose2d.kZero; } - public default Command currentZeroTurret() { - return Commands.none(); - } + public default Command currentZeroTurret() { + return Commands.none(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 03b8b667..828096fb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -56,10 +56,9 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final double FLYWHEEL_DIAMETER_INCHES = 4; public static final Rotation2d TURRET_LEFT_FIXED_SHOT_ANGLE = Rotation2d.kZero; - public static final Rotation2d TURRET_RIGHT_FIXED_SHOT_ANGLE = Rotation2d.kZero; + public static final Rotation2d TURRET_RIGHT_FIXED_SHOT_ANGLE = Rotation2d.kZero; public static final Rotation2d TURRET_MID_FIXED_SHOT_ANGLE = Rotation2d.kZero; - // TODO: REDO THIS HARDSTOP WHEN FIXED?? // logged for ease of graph viewing @AutoLogOutput(key = "Shooter/Turret/Rear Hardstop") @@ -472,19 +471,19 @@ public Command score( hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(Rotation2d.kZero); // TODO + turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); // in front of tower with intake facing left (to avoid deadzone) case MID: hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(Rotation2d.kZero); // TODO + turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); // in front of right trench with intake facing alliance wall case RIGHT: hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(Rotation2d.kZero); // TODO + turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); case NONE: hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index cd04d464..9216f1bd 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -23,6 +23,10 @@ public class AutoAim { public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); + public static final Rotation2d LEFT_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-73.916016); + public static final Rotation2d MID_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-82); + public static final Rotation2d RIGHT_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-109.775391); + static { // For hub shot tree ALPHA_HUB_SHOT_TREE.put( Units.inchesToMeters(24 + 17), new ShotData(Rotation2d.fromDegrees(8), 27.5, 1.46)); @@ -257,14 +261,14 @@ public static boolean targetInTurretDeadzone() { } public static ShotData getLeftFixedShotData() { - return new ShotData(Rotation2d.kZero, 0, 0); + return new ShotData(Rotation2d.fromDegrees(36), 36, 0); } public static ShotData getRightFixedShotData() { - return new ShotData(Rotation2d.kZero, 0, 0); + return new ShotData(Rotation2d.fromDegrees(23.16), 35.7, 0); } public static ShotData getMidFixedShotData() { - return new ShotData(Rotation2d.kZero, 0, 0); + return new ShotData(Rotation2d.fromDegrees(32.84), 35, 0); } } From 068a5e0e7521c5f55ce93b5d24ca2ad06cab5aa0 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 13 Mar 2026 13:07:58 -0700 Subject: [PATCH 6/6] fmt --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index febde77b..7fd28fae 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -26,7 +26,8 @@ public class AutoAim { public static final Rotation2d LEFT_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-73.916016); public static final Rotation2d MID_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-82); - public static final Rotation2d RIGHT_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-109.775391); + public static final Rotation2d RIGHT_FIXED_SHOT_TURRET_ANGLE = + Rotation2d.fromDegrees(-109.775391); static { // For hub shot tree ALPHA_HUB_SHOT_TREE.put(