Add extension collision checker

src/main/java/frc/robot/Robot.java

@@ -60,6 +60,8 @@
 import frc.robot.subsystems.vision.VisionIOSim;
 import frc.robot.subsystems.wrist.*;
 import frc.robot.utils.CommandXboxControllerSubsystem;
+import frc.robot.utils.LoggedTunableBoolean;
+import frc.robot.utils.LoggedTunableNumber;
 import frc.robot.utils.Tracer;
 import frc.robot.utils.autoaim.AutoAim;
 import frc.robot.utils.autoaim.AutoAimTargets;
@@ -332,6 +334,16 @@ public static enum AlgaeScoreTarget {
   // Main benefit to that is reducing startup time, which idt we care about too much
   private final LoggedDashboardChooser<Command> autoChooser = new LoggedDashboardChooser<>("Autos");
 
+  public static final boolean TUNING_MODE = true;
+  private final LoggedTunableNumber elevatorManualTarget =
+      new LoggedTunableNumber("Manual Target/Elevator", 0.0);
+  private final LoggedTunableNumber shoulderManualTarget =
+      new LoggedTunableNumber("Manual Target/Shoulder", 0.0);
+  private final LoggedTunableNumber wristManualTarget =
+      new LoggedTunableNumber("Manual Target/Wrist", 0.0);
+  private final LoggedTunableBoolean manualTarget =
+      new LoggedTunableBoolean("Manual Target/Enable", false);
+
   // Mechanisms
   private final LoggedMechanism2d elevatorMech2d =
       new LoggedMechanism2d(3.0, Units.feetToMeters(4.0));
@@ -546,6 +558,16 @@ public Robot() {
         .rightTrigger()
         .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.PROCESSOR));
 
+    new Trigger(() -> manualTarget.get())
+        // .and(() -> superstructure.getState() == SuperState.IDLE)
+        .onTrue(Commands.print("Manual Control"))
+        .whileTrue(
+            Commands.parallel(
+                Commands.print("Manual Control Activated"),
+                elevator.setExtension(elevatorManualTarget::get),
+                shoulder.setTargetAngle(() -> Rotation2d.fromDegrees(shoulderManualTarget.get())),
+                wrist.setTargetAngle(() -> Rotation2d.fromDegrees(wristManualTarget.get()))));
+
     // Log locations of all autoaim targets
     Logger.recordOutput(
         "AutoAim/Targets",
@@ -593,6 +615,7 @@ public void robotPeriodic() {
           "MapleSim/Pose", swerveDriveSimulation.get().getSimulatedDriveTrainPose());
     }
 
+    Logger.recordOutput("Manual Override", manualTarget.get());
     Logger.recordOutput("Targets/Reef Target", currentTarget);
     Logger.recordOutput("Targets/Algae Intake Target", algaeIntakeTarget);
     Logger.recordOutput("Targets/Algae Score Target", algaeScoreTarget);

src/main/java/frc/robot/subsystems/Superstructure.java

@@ -1,8 +1,12 @@
 package frc.robot.subsystems;
 
+import com.google.common.collect.Streams;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -20,6 +24,7 @@
 import java.util.HashMap;
 import java.util.Map;
 import java.util.function.Supplier;
+import java.util.stream.Stream;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -152,6 +157,11 @@ public Superstructure(
   /** This file is not a subsystem, so this MUST be called manually. */
   public void periodic() {
     Logger.recordOutput("Superstructure/Superstructure State", state);
+    if (Robot.isSimulation()) {
+      Logger.recordOutput("Superstructure/Check Collisions", checkCollisions());
+      Logger.recordOutput(
+          "Superstructure/Collision Statics", getStaticColliders(elevator.getExtensionMeters()));
+    }
   }
 
   private void configureStateTransitionCommands() {
@@ -579,4 +589,110 @@ private Command forceState(SuperState nextState) {
           this.state = nextState;
         });
   }
+
+  public boolean checkCollisions() {
+    return checkCollisions(elevator.getExtensionMeters(), shoulder.getAngle(), wrist.getAngle());
+  }
+
+  public static boolean checkCollisions(
+      double elevatorExtension, Rotation2d shoulderAngle, Rotation2d wristAngle) {
+    // Check if arm collides with hp, cross bar, bumper
+    final var armRect =
+        new Rectangle2d(
+            new Pose2d( // arm
+                new Translation2d(
+                    ShoulderSubsystem.X_OFFSET_METERS
+                        + shoulderAngle.getCos() * ShoulderSubsystem.ARM_LENGTH_METERS / 2.0,
+                    ShoulderSubsystem.Z_OFFSET_METERS
+                        + elevatorExtension
+                        + shoulderAngle.getSin() * ShoulderSubsystem.ARM_LENGTH_METERS / 2.0),
+                new Rotation2d(
+                    (Math.PI / 2) + Units.degreesToRadians(2.794042) + shoulderAngle.getRadians())),
+            Units.inchesToMeters(2.787574),
+            ShoulderSubsystem.ARM_LENGTH_METERS);
+    // Check if back of manip collides w ..
+    final var manipRect =
+        new Rectangle2d(
+            new Pose2d( // Manipulator
+                new Translation2d(
+                        ShoulderSubsystem.X_OFFSET_METERS
+                            + shoulderAngle.getCos() * ShoulderSubsystem.ARM_LENGTH_METERS,
+                        elevatorExtension
+                            + ShoulderSubsystem.Z_OFFSET_METERS
+                            + shoulderAngle.getSin() * ShoulderSubsystem.ARM_LENGTH_METERS)
+                    .plus(
+                        new Translation2d(0.0, Units.inchesToMeters(8.0 / 2)).rotateBy(wristAngle)),
+                new Rotation2d(wristAngle.getRadians())),
+            Units.inchesToMeters(1.5),
+            Units.inchesToMeters(8.0));
+    // Check if centering of manip collides w ..
+    final var centeringRect =
+        new Rectangle2d(
+            manipRect.getCenter(), Units.inchesToMeters(3.0 * 2), Units.inchesToMeters(1.0));
+    // Check if tusk collides w ..
+    final var tuskRect =
+        new Rectangle2d(
+            new Pose2d( // Manipulator
+                new Translation2d(
+                        ShoulderSubsystem.X_OFFSET_METERS
+                            + shoulderAngle.getCos() * ShoulderSubsystem.ARM_LENGTH_METERS,
+                        elevatorExtension
+                            + ShoulderSubsystem.Z_OFFSET_METERS
+                            + shoulderAngle.getSin() * ShoulderSubsystem.ARM_LENGTH_METERS)
+                    .plus(
+                        new Translation2d(Units.inchesToMeters(15.0), Units.inchesToMeters(-3.5))
+                            .rotateBy(wristAngle)),
+                new Rotation2d(wristAngle.getRadians())),
+            Units.inchesToMeters(3.0),
+            Units.inchesToMeters(3.0));
+    Logger.recordOutput(
+        "Collision Boxes",
+        Streams.concat(
+                Stream.of(getPoints(armRect)),
+                Stream.of(getPoints(manipRect)),
+                Stream.of(getPoints(centeringRect)),
+                Stream.of(getPoints(tuskRect)))
+            .toArray(Translation2d[]::new));
+    return checkCollision(armRect, elevatorExtension)
+        || checkCollision(manipRect, elevatorExtension)
+        || checkCollision(centeringRect, elevatorExtension)
+        || checkCollision(tuskRect, elevatorExtension);
+  }
+
+  private static Translation2d[] getPoints(Rectangle2d rect) {
+    return new Translation2d[] {
+      new Translation2d(rect.getXWidth() / 2.0, rect.getYWidth() / 2.0)
+          .rotateBy(rect.getRotation())
+          .plus(rect.getCenter().getTranslation()),
+      new Translation2d(-rect.getXWidth() / 2.0, rect.getYWidth() / 2.0)
+          .rotateBy(rect.getRotation())
+          .plus(rect.getCenter().getTranslation()),
+      new Translation2d(rect.getXWidth() / 2.0, -rect.getYWidth() / 2.0)
+          .rotateBy(rect.getRotation())
+          .plus(rect.getCenter().getTranslation()),
+      new Translation2d(-rect.getXWidth() / 2.0, -rect.getYWidth() / 2.0)
+          .rotateBy(rect.getRotation())
+          .plus(rect.getCenter().getTranslation())
+    };
+  }
+
+  private static Translation2d[] getStaticColliders(double elevatorExtension) {
+    return new Translation2d[] {
+      // funnel
+      new Translation2d(Units.inchesToMeters(2.173), Units.inchesToMeters(23.368176)),
+      // crossbar
+      new Translation2d(Units.inchesToMeters(0.7), Units.inchesToMeters(37.683)),
+      new Translation2d(
+          Units.inchesToMeters(3.000), Units.inchesToMeters(43.557883) + elevatorExtension / 2.0),
+      // bumpers
+      new Translation2d(Units.inchesToMeters(14.850000), Units.inchesToMeters(5.698368)),
+      new Translation2d(Units.inchesToMeters(14.85 + 3.0), Units.inchesToMeters(5.698368))
+    };
+  }
+
+  /** Checks if a rectangle is colliding with the funnel, crossbar, or bumpers */
+  private static boolean checkCollision(Rectangle2d rect, double elevatorExtension) {
+    return Stream.of(getStaticColliders(elevatorExtension))
+        .anyMatch((c) -> rect.getDistance(c) <= Units.inchesToMeters(0.0));
+  }
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -36,7 +36,7 @@ public class ElevatorSubsystem extends SubsystemBase {
   public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(19.75);
   public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.25);
 
-  public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(50.0);
+  public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(63.5);
   public static final double ALGAE_PROCESSOR_EXTENSION = Units.inchesToMeters(10.0);
 
   public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0);

src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java

@@ -27,7 +27,7 @@ public class ShoulderSubsystem extends SubsystemBase {
   public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(35);
   public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(68.5);
   public static final Rotation2d SHOULDER_SCORE_POS = Rotation2d.fromDegrees(75);
-  public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90);
+  public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(85);
   // TODO: SET TO CORRECT POS
   public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(0.0);
 

src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java

@@ -26,7 +26,7 @@ public class WristSubsystem extends SubsystemBase {
   public static final Rotation2d WRIST_SCORE_L3_POS = Rotation2d.fromDegrees(-35);
   public static final Rotation2d WRIST_SCORE_L4_POS = Rotation2d.fromDegrees(-40);
   public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-33.5);
-  public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(75);
+  public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(90);
   public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-45.0);
 
   private final WristIO io;

src/main/java/frc/robot/utils/LoggedTunableBoolean.java

@@ -0,0 +1,124 @@
+// Copyright (c) 2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.utils;
+
+import frc.robot.Robot;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class LoggedTunableBoolean implements BooleanSupplier {
+  private static final String tableKey = "/Tuning";
+
+  private final String key;
+  private boolean hasDefault = false;
+  private boolean defaultValue;
+  private LoggedNetworkBoolean dashboardBoolean;
+  private Map<Integer, Boolean> lastHasChangedValues = new HashMap<>();
+
+  /**
+   * Create a new LoggedTunableNumber
+   *
+   * @param dashboardKey Key on dashboard
+   */
+  public LoggedTunableBoolean(String dashboardKey) {
+    this.key = tableKey + "/" + dashboardKey;
+  }
+
+  /**
+   * Create a new LoggedTunableNumber with the default value
+   *
+   * @param dashboardKey Key on dashboard
+   * @param defaultValue Default value
+   */
+  public LoggedTunableBoolean(String dashboardKey, boolean defaultValue) {
+    this(dashboardKey);
+    initDefault(defaultValue);
+  }
+
+  /**
+   * Set the default value of the number. The default value can only be set once.
+   *
+   * @param defaultValue The default value
+   */
+  public void initDefault(boolean defaultValue) {
+    if (!hasDefault) {
+      hasDefault = true;
+      this.defaultValue = defaultValue;
+      if (Robot.TUNING_MODE) {
+        dashboardBoolean = new LoggedNetworkBoolean(key, defaultValue);
+      }
+    }
+  }
+
+  /**
+   * Get the current value, from dashboard if available and in tuning mode.
+   *
+   * @return The current value
+   */
+  public boolean get() {
+    if (!hasDefault) {
+      return false;
+    } else {
+      return Robot.TUNING_MODE ? dashboardBoolean.get() : defaultValue;
+    }
+  }
+
+  /**
+   * Checks whether the number has changed since our last check
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @return True if the number has changed since the last time this method was called, false
+   *     otherwise.
+   */
+  public boolean hasChanged(int id) {
+    boolean currentValue = get();
+    Boolean lastValue = lastHasChangedValues.get(id);
+    if (lastValue == null || currentValue != lastValue) {
+      lastHasChangedValues.put(id, currentValue);
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * Runs action if any of the tunableNumbers have changed
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+   *     numbers in order inputted in method
+   * @param tunableBooleans All tunable numbers to check
+   */
+  public static void ifChanged(
+      int id, Consumer<Boolean[]> action, LoggedTunableBoolean... tunableBooleans) {
+    if (Arrays.stream(tunableBooleans).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+      action.accept(
+          Arrays.stream(tunableBooleans).map(LoggedTunableBoolean::get).toArray(Boolean[]::new));
+    }
+  }
+
+  /** Runs action if any of the tunableNumbers have changed */
+  public static void ifChanged(int id, Runnable action, LoggedTunableBoolean... tunableNumbers) {
+    ifChanged(id, values -> action.run(), tunableNumbers);
+  }
+
+  @Override
+  public boolean getAsBoolean() {
+    return get();
+  }
+}