Added a bunch of the limelight stuff

src/main/java/frc/robot/Constants.java

@@ -18,4 +18,5 @@ public final class Constants {
   public static final float PLACEHOLDER_FLOAT = 0;
 
   public static final int NUM_CANDLE_LEDS = PLACEHOLDER_INT;
+  public static final double FIELD_WIDTH_METERS = 8.02;
 }

src/main/java/frc/robot/RobotContainer.java

@@ -63,7 +63,7 @@ public class RobotContainer {
   private ClawLimelight clawLimelight = new ClawLimelight();
   private Grabber grabber = new Grabber(rumbleBriefly);
   private Lights lights = new Lights();
-  private TagLimelight tagLimelight = new TagLimelight();
+  private TagLimelight tagLimelight = new TagLimelight(scoreLoc);
   public DriveSubsystem drive = new DriveSubsystem(lights, () -> timedMatch);
 
   // A chooser for autonomous commands

src/main/java/frc/robot/commands/LookForTag.java

@@ -0,0 +1,51 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Lights;
+import frc.robot.subsystems.Lights.LightCode;
+import frc.robot.subsystems.TagLimelight;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import java.util.Optional;
+
+public class LookForTag extends CommandBase{
+    private DriveSubsystem drive;
+    private TagLimelight tagLimelight;
+    private Lights lights;
+    private boolean targetLocked = false;
+
+    public LookForTag(
+        TagLimelight limelight, DriveSubsystem driveSubsystem, Lights lightsSubsysten) {
+        drive = driveSubsystem;
+        tagLimelight = limelight;
+        lights = lightsSubsysten;
+    }
+
+    @Override
+    public void initialize() {
+        lights.toggleCode(LightCode.NO_TAG);
+    }
+
+    @Override
+    public void execute() {
+        if (!targetLocked) {
+        Optional<Transform2d> robotToScoringLocation = tagLimelight.checkForTag();
+        if (!robotToScoringLocation.isPresent()) {
+            robotToScoringLocation = Optional.empty();
+            return;
+        }
+        double latencySeconds = tagLimelight.getLatencySeconds();
+        targetLocked = true;
+        drive.setLimelightTargetFromTransform(robotToScoringLocation.get(), latencySeconds);
+        lights.toggleCode(LightCode.OFF);
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {}
+
+    @Override
+    public boolean isFinished() {
+        return targetLocked;
+    }
+}

src/main/java/frc/robot/commands/SwerveFollowerWrapper.java

@@ -0,0 +1,54 @@
+package frc.robot.commands;
+
+import com.pathplanner.lib.PathPlannerTrajectory;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import java.util.Optional;
+
+public class SwerveFollowerWrapper extends CommandBase {
+
+  public Command swerveFollowerCmd;
+  private DriveSubsystem drive;
+  boolean noTrajectory = true;
+
+  public SwerveFollowerWrapper(DriveSubsystem driveSubsystem) {
+    drive = driveSubsystem;
+    addRequirements(drive);
+  }
+
+  @Override
+  public void initialize() {
+    Optional<PathPlannerTrajectory> maybeTrajectory = drive.poseToPath();
+    if (!maybeTrajectory.isPresent()) {
+      return;
+    }
+    noTrajectory = false;
+    swerveFollowerCmd =
+        drive.followTrajectoryCommand(maybeTrajectory.get(), false).withTimeout(3.0);
+    swerveFollowerCmd.initialize();
+  }
+
+  @Override
+  public void execute() {
+    if (!noTrajectory) {
+      swerveFollowerCmd.execute();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    if (!noTrajectory) {
+      swerveFollowerCmd.end(interrupted);
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (!noTrajectory) {
+      return swerveFollowerCmd.isFinished();
+    } else {
+      return true;
+    }
+  }
+}

src/main/java/frc/robot/commands/SwerveToPointWrapper.java

@@ -0,0 +1,64 @@
+package frc.robot.commands;
+
+import com.pathplanner.lib.PathPlannerTrajectory;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import java.util.function.Supplier;
+
+public class SwerveToPointWrapper extends CommandBase {
+
+  public Command swerveFollowerCmd;
+  private DriveSubsystem drive;
+  private Supplier<Pose2d> desiredPoseSupplier;
+  private double finalSpeedMetersPerSec;
+  boolean flipForRed;
+  double timeoutSec;
+
+  public SwerveToPointWrapper(
+      boolean red,
+      DriveSubsystem driveSubsystem,
+      Supplier<Pose2d> finalPoseSupplier,
+      double timeoutSeconds,
+      double endSpeedMps) {
+    flipForRed = red;
+    desiredPoseSupplier = finalPoseSupplier;
+    timeoutSec = timeoutSeconds;
+    finalSpeedMetersPerSec = endSpeedMps;
+    drive = driveSubsystem;
+    addRequirements(drive);
+  }
+
+  @Override
+  public void initialize() {
+    Pose2d finalPose = desiredPoseSupplier.get();
+    if (flipForRed) {
+      finalPose =
+          new Pose2d(
+              finalPose.getX(),
+              Constants.FIELD_WIDTH_METERS - finalPose.getY(),
+              finalPose.getRotation());
+    }
+
+    PathPlannerTrajectory trajectory = drive.pathToPoint(finalPose, finalSpeedMetersPerSec);
+    swerveFollowerCmd = drive.followTrajectoryCommand(trajectory, false).withTimeout(timeoutSec);
+    swerveFollowerCmd.initialize();
+  }
+
+  @Override
+  public void execute() {
+    swerveFollowerCmd.execute();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    swerveFollowerCmd.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return swerveFollowerCmd.isFinished();
+  }
+}

src/main/java/frc/robot/commands/TeleopDriveToTag.java

@@ -0,0 +1,21 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.Lights;
+import frc.robot.subsystems.Lights.LightCode;
+import frc.robot.subsystems.TagLimelight;
+import frc.robot.subsystems.drive.DriveSubsystem;
+
+public class TeleopDriveToTag extends SequentialCommandGroup {
+
+  public TeleopDriveToTag(TagLimelight limelight, DriveSubsystem drive, Lights lights) {
+    addCommands(
+        new LookForTag(limelight, drive, lights),
+        new SwerveFollowerWrapper(drive).withTimeout(2.0).asProxy(),
+        new InstantCommand(
+            () -> {
+              lights.toggleCode(LightCode.READY_TO_SCORE);
+            }));
+  }
+}

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

@@ -1,5 +1,152 @@
 package frc.robot.subsystems;
 
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.utils.LimelightHelpers;
+import frc.robot.utils.LimelightHelpers.LimelightTarget_Fiducial;
+import frc.robot.utils.ScoringLocationUtil;
+import java.util.Optional;
 
-public class TagLimelight extends SubsystemBase {}
+public class TagLimelight extends SubsystemBase {
+  private ScoringLocationUtil scoreLoc;
+
+  private Optional<Transform2d> robotToScoringLocation = Optional.empty();
+
+  public TagLimelight(ScoringLocationUtil scoringLocUtil) {
+    scoreLoc = scoringLocUtil;
+
+    LimelightHelpers.setLEDMode_ForceOff("");
+  }
+
+  private static Transform2d getBotFromTarget(Pose3d botPoseTargetSpace) {
+    /**
+     * Target space: 3d Cartesian Coordinate System with (0,0,0) at the center of the target.
+     *
+     * <p>X+ → Pointing to the right of the target (If you are looking at the target)
+     *
+     * <p>Y+ → Pointing downward
+     *
+     * <p>Z+ → Pointing out of the target (orthogonal to target’s plane).
+     */
+
+    /**
+     * We convert to 2d target space: X+ -> Out of the target Y+ -> Pointing to the right of the
+     * target (If you are looking at the target) This means positive yaw is based on Z+ being up
+     */
+    Translation2d translation =
+        new Translation2d(botPoseTargetSpace.getZ(), botPoseTargetSpace.getX());
+    // Rotation2d rot = Rotation2d.fromDegrees(-botPoseTargetSpace.getRotation().getY());
+
+    Rotation2d rot = Rotation2d.fromDegrees(0);
+    System.out.println("Tag at " + -botPoseTargetSpace.getRotation().getY() + " deg");
+    return new Transform2d(translation, rot);
+  }
+
+  private static boolean validScoringTag(double tagId) {
+    long tagIdRounded = Math.round(tagId);
+    if (tagIdRounded == 1
+        || tagIdRounded == 2
+        || tagIdRounded == 3
+        || tagIdRounded == 6
+        || tagIdRounded == 7
+        || tagIdRounded == 8) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  public static int chooseTag(LimelightTarget_Fiducial[] targets) {
+    int numTags = targets.length;
+    double minDistMeters = Double.MAX_VALUE;
+    int bestTag = 0;
+    for (int tagIndex = 0; tagIndex < numTags; tagIndex++) {
+      LimelightTarget_Fiducial target = targets[tagIndex];
+      if (!validScoringTag(target.fiducialID)) {
+        continue;
+      }
+      double targetDistance = target.getTargetPose_RobotSpace().getTranslation().getNorm();
+      if (targetDistance < minDistMeters) {
+        minDistMeters = targetDistance;
+        bestTag = tagIndex;
+      }
+    }
+    return bestTag;
+  }
+
+  private Transform2d getRobotToScoringLocation(Pose3d targetPoseRobotSpace) {
+    Transform2d targetFromBot = getBotFromTarget(targetPoseRobotSpace);
+    Transform2d scoringLocationFromTag = scoreLoc.scoringLocationFromTag();
+    return targetFromBot.plus(scoringLocationFromTag);
+  }
+
+  public Optional<Transform2d> getRobotToScoringLocation() {
+    return robotToScoringLocation;
+  }
+
+  public Optional<Transform2d> checkForTag() {
+    if (!LimelightHelpers.getTV("")) {
+      robotToScoringLocation = Optional.empty();
+      return Optional.empty();
+    }
+    if (!validScoringTag(LimelightHelpers.getFiducialID(""))) {
+      // TODO for now, allow all grid tags for scoring
+      // if (LimelightHelpers.getFiducialID("") != 6.0 && LimelightHelpers.getFiducialID("") != 3.0)
+      // {
+      robotToScoringLocation = Optional.empty();
+      return Optional.empty();
+    }
+    robotToScoringLocation =
+        Optional.of(getRobotToScoringLocation(LimelightHelpers.getTargetPose3d_RobotSpace("")));
+    return robotToScoringLocation;
+  }
+
+  public double getLatencySeconds() {
+    return (LimelightHelpers.getLatency_Capture("") + LimelightHelpers.getLatency_Pipeline(""))
+        / 1000.0;
+  }
+
+  @Override
+  public void periodic() {
+    // checkForTag();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty(
+        "Translation X (m)",
+        () -> {
+          if (robotToScoringLocation.isPresent()) {
+            return String.valueOf(robotToScoringLocation.get().getX());
+          } else {
+            return "empty";
+          }
+        },
+        null);
+    builder.addStringProperty(
+        "Translation Y (m)",
+        () -> {
+          if (robotToScoringLocation.isPresent()) {
+            return String.valueOf(robotToScoringLocation.get().getY());
+          } else {
+            return "empty";
+          }
+        },
+        null);
+    builder.addStringProperty(
+        "Rotation (deg)",
+        () -> {
+          if (robotToScoringLocation.isPresent()) {
+            return String.valueOf(robotToScoringLocation.get().getRotation().getDegrees());
+          } else {
+            return "empty";
+          }
+        },
+        null);
+  }
+}