HighlanderRobotics · spellingcat · Mar 13, 2026 · Mar 10, 2026 · Mar 10, 2026 · Mar 10, 2026
diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
@@ -76,6 +76,13 @@ public enum FeedTarget {
     RIGHT
   }
 
+  public enum FixedShotTarget {
+    LEFT,
+    MID,
+    RIGHT,
+    NONE
+  }
+
   @AutoLogOutput(key = "Superstructure/State")
   private static SuperState state = SuperState.IDLE;
 
@@ -119,7 +126,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")
@@ -140,9 +147,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,
@@ -190,11 +200,18 @@ 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()
             .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);
@@ -311,7 +328,10 @@ private void addTransitions() {
     (preClimbReq.and(climbReq.negate()).and(() -> DriverStation.isTeleop()))
         .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()));
 
@@ -746,11 +766,19 @@ public boolean canScore() {
         && !swerve.isNearTrench();
   }
 
+  public boolean canShoot() {
+    return !swerve.isNearTrenchForHood();
+  }
+
   public static ShotTarget getShotTarget() {
     return shotTarget;
   }
 
   public static FeedTarget getFeedTarget() {
     return feedTarget;
   }
+
+  public static FixedShotTarget getFixedShotTarget() {
+    return fixedShotTarget;
+  }
 }
diff --git 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
@@ -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
@@ -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;
@@ -50,9 +51,14 @@ 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;
 
+  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")
@@ -72,7 +78,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;
@@ -182,7 +189,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);
@@ -398,7 +405,7 @@ public Command zeroHood() {
   public Command runHoodCurrentZeroing() {
     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()));
   }
@@ -458,15 +465,35 @@ public Command score(
       Supplier<ChassisSpeeds> 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(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(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(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE);
+            case NONE:
+              hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle());
+              flywheelIO.setMotionProfiledFlywheelVelocity(
+                  shotDataSupplier.get().flywheelVelocityRotPerSec());
+              turretIO.setTurretPosition(
+                  AutoAim.getTurretHubTargetRotation(
+                      FieldUtils.getCurrentHubTranslation(),
+                      robotPoseSupplier.get(),
+                      chassisSpeedsSupplier.get()));
+          }
         });
   }
 
@@ -639,19 +666,54 @@ public Command spinUp(
       Supplier<ChassisSpeeds> 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(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(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(TURRET_RIGHT_FIXED_SHOT_ANGLE); // TODO
+            case NONE:
+              hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle());
+              flywheelIO.setMotionProfiledFlywheelVelocity(
+                  shotDataSupplier.get().flywheelVelocityRotPerSec());
+              turretIO.setTurretPosition(
+                  AutoAim.getTurretHubTargetRotation(
+                      FieldUtils.getCurrentHubTranslation(),
+                      robotPoseSupplier.get(),
+                      chassisSpeedsSupplier.get()));
+          }
         });
   }
 
   @Override
   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));
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -818,6 +818,20 @@ public boolean isNearTrench() {
     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 isNearBump() {
     double x = getPose().getX();
     double y = getPose().getY();

diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -24,6 +24,11 @@ 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));
@@ -256,4 +261,16 @@ public static ShotData getCompensatedSOTMShotData(
   public static boolean targetInTurretDeadzone() {
     return outOfRange;
   }
+
+  public static ShotData getLeftFixedShotData() {
+    return new ShotData(Rotation2d.fromDegrees(36), 36, 0);
+  }
+
+  public static ShotData getRightFixedShotData() {
+    return new ShotData(Rotation2d.fromDegrees(23.16), 35.7, 0);
+  }
+
+  public static ShotData getMidFixedShotData() {
+    return new ShotData(Rotation2d.fromDegrees(32.84), 35, 0);
+  }
 }