Note detection

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

@@ -41,6 +41,8 @@
 import frc.robot.subsystems.swerve.GyroIOPigeon2;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem.AutoAimStates;
+import frc.robot.subsystems.vision.NoteDetectionIOReal;
+import frc.robot.subsystems.vision.NoteDetectionIOSim;
 import frc.robot.utils.CommandXboxControllerSubsystem;
 import frc.robot.utils.autoaim.AutoAim;
 import java.util.function.Supplier;
@@ -92,7 +94,10 @@ public void setYaw(Rotation2d yaw) {}
               : SwerveSubsystem.createSimCameras(),
           mode == RobotMode.REAL
               ? SwerveSubsystem.createTalonFXModules()
-              : SwerveSubsystem.createSimModules());
+              : SwerveSubsystem.createSimModules(),
+          mode == RobotMode.REAL
+              ? new NoteDetectionIOReal(SwerveSubsystem.noteDetectionCamAConstants)
+              : new NoteDetectionIOSim(SwerveSubsystem.noteDetectionCamAConstants));
   private final IntakeSubsystem intake = new IntakeSubsystem(new IntakeIOReal());
   private final FeederSubsystem feeder = new FeederSubsystem(new FeederIOReal());
   private final ElevatorSubsystem elevator =

src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java

@@ -32,6 +32,7 @@
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -65,6 +66,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.FieldConstants;
 import frc.robot.subsystems.swerve.Module.ModuleConstants;
+import frc.robot.subsystems.vision.NoteDetectionIO;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.Vision.VisionConstants;
 import frc.robot.subsystems.vision.VisionHelper;
@@ -73,6 +75,7 @@
 import frc.robot.subsystems.vision.VisionIOSim;
 import frc.robot.utils.autoaim.AutoAim;
 import frc.robot.utils.autoaim.ShotData;
+import frc.robot.utils.vision.NoteDetectionIOInputsLogged;
 import java.util.Arrays;
 import java.util.NoSuchElementException;
 import java.util.concurrent.locks.Lock;
@@ -212,12 +215,21 @@ public class AutoAimStates {
               new Rotation3d(0, -0.490, Math.PI * 2 / 3)), // TODO check
           RIGHT_CAMERA_MATRIX_OPT,
           RIGHT_DIST_COEFFS_OPT);
+  public static final VisionConstants noteDetectionCamAConstants =
+      new VisionConstants(
+          "a",
+          new Transform3d(new Translation3d(), new Rotation3d()), // TODO
+          LEFT_CAMERA_MATRIX,
+          LEFT_DIST_COEFFS);
   private SwerveDriveOdometry odometry;
 
   private final SysIdRoutine moduleSteerRoutine;
   private final SysIdRoutine driveRoutine;
+  private final NoteDetectionIO noteDetectionIO; // TODO assuming there is only one for now
+  private final NoteDetectionIOInputsLogged noteDetectionInputs = new NoteDetectionIOInputsLogged();
 
-  public SwerveSubsystem(GyroIO gyroIO, VisionIO[] visionIOs, ModuleIO[] moduleIOs) {
+  public SwerveSubsystem(
+      GyroIO gyroIO, VisionIO[] visionIOs, ModuleIO[] moduleIOs, NoteDetectionIO noteDetectionIO) {
     this.gyroIO = gyroIO;
     cameras = new Vision[visionIOs.length];
     new AutoAim();
@@ -230,6 +242,7 @@ public SwerveSubsystem(GyroIO gyroIO, VisionIO[] visionIOs, ModuleIO[] moduleIOs
     for (int i = 0; i < visionIOs.length; i++) {
       cameras[i] = new Vision(visionIOs[i]);
     }
+    this.noteDetectionIO = noteDetectionIO;
 
     // mildly questionable
     VisionIOSim.pose = this::getPose3d;
@@ -356,6 +369,8 @@ public void periodic() {
     for (var module : modules) {
       module.periodic();
     }
+    noteDetectionIO.updateInputs(noteDetectionInputs);
+    Logger.processInputs("Note Detection", noteDetectionInputs);
 
     // Stop moving when disabled
     if (DriverStation.isDisabled()) {
@@ -876,4 +891,36 @@ public Command runDriveCharacterizationCmd() {
         driveRoutine.dynamic(Direction.kReverse),
         this.runOnce(() -> SignalLogger.stop()));
   }
+
+  // TODO idk if i did this right
+  public Command poseLockDriveCmd(DoubleSupplier x, DoubleSupplier y, DoubleSupplier theta) {
+    PIDController xController = new PIDController(-1, 0, 0);
+    PIDController yController = new PIDController(-1, 0, 0);
+    PIDController headingController = new PIDController(1.2, 0, 0.1);
+    headingController.enableContinuousInput(0, 2 * Math.PI);
+    return runVelocityFieldRelative(
+        () -> {
+          return new ChassisSpeeds(
+              xController.calculate(pose.getX(), x.getAsDouble()) * MAX_LINEAR_SPEED,
+              yController.calculate(pose.getY(), y.getAsDouble()) * MAX_LINEAR_SPEED,
+              headingController.calculate(pose.getRotation().getRadians(), theta.getAsDouble())
+                  * MAX_ANGULAR_SPEED);
+        });
+  }
+
+  public Command driveToPoseCmd(Supplier<Pose2d> pose) {
+    return poseLockDriveCmd(
+        () -> pose.get().getX(),
+        () -> pose.get().getY(),
+        () -> pose.get().getRotation().getRadians());
+  }
+
+  public Command driveToNoteCmd() {
+      var note = VisionHelper.getBestTarget(getPose(), new PhotonPipelineResult(noteDetectionInputs.latency, noteDetectionInputs.targets), 0); //TODO change camerax and ppresult
+      if (note.isEmpty()) {
+        return Commands.none();
+      } // TODO driver feedback? Must be proxied for duration
+      var pickup = note.get().transformBy(new Transform2d(new Translation2d(-1, 0), new Rotation2d()));
+      return driveToPoseCmd(() -> pickup);
+    }
 }

src/main/java/frc/robot/subsystems/vision/NoteDetectionIO.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.vision;
+
+import frc.robot.subsystems.vision.Vision.VisionConstants;
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+/** Add your docs here. */
+public interface NoteDetectionIO { //TODO make vision superinterface/class?
+
+  public static class NoteDetectionIOInputs {
+    public double timestamp = 0.0;
+    public double latency = 0.0;
+    public List<PhotonTrackedTarget> targets = new ArrayList<>(); // TODO protobuf
+    public double numTargets = 0;
+    public VisionConstants constants;
+    public double yaw = 0.0;
+    public double pitch = 0.0;
+    public double distance = 0.0;
+  }
+
+  public void updateInputs(NoteDetectionIOInputs inputs);
+
+  public String getName();
+}

src/main/java/frc/robot/subsystems/vision/NoteDetectionIOReal.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.vision.Vision.VisionConstants;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonUtils;
+
+/** Add your docs here. */
+public class NoteDetectionIOReal implements NoteDetectionIO {
+  public String cameraName;
+  public PhotonCamera camera;
+  public Matrix<N3, N3> cameraMatrix;
+  public Matrix<N5, N1> distCoeffs;
+
+  private final VisionConstants constants;
+
+  public NoteDetectionIOReal(VisionConstants constants) {
+    cameraName = constants.cameraName();
+    camera = new PhotonCamera(cameraName);
+    cameraMatrix = constants.intrinsicsMatrix();
+    distCoeffs = constants.distCoeffs();
+    this.constants = constants;
+  }
+
+  @Override
+  public void updateInputs(NoteDetectionIOInputs inputs) {
+    var result = camera.getLatestResult();
+    inputs.timestamp = result.getTimestampSeconds();
+    inputs.latency = result.getLatencyMillis();
+    inputs.targets = result.targets;
+    inputs.constants = constants;
+    inputs.yaw = -Units.degreesToRadians(result.getBestTarget().getYaw()); // why is this negative
+    inputs.pitch = -Units.degreesToRadians(result.getBestTarget().getPitch());
+    inputs.distance =
+        PhotonUtils.calculateDistanceToTargetMeters(
+            0, 0, 0, Units.degreesToRadians(result.getBestTarget().getPitch())); // TODO fix
+  }
+
+  @Override
+  public String getName() {
+    return cameraName;
+  }
+}

src/main/java/frc/robot/subsystems/vision/NoteDetectionIOSim.java

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
+import frc.robot.subsystems.vision.Vision.VisionConstants;
+import org.photonvision.PhotonCamera;
+
+/** Add your docs here. */
+public class NoteDetectionIOSim implements NoteDetectionIO {
+  public String cameraName;
+  public PhotonCamera camera;
+  public Matrix<N3, N3> cameraMatrix;
+  public Matrix<N5, N1> distCoeffs;
+
+  private final VisionConstants constants;
+
+  public NoteDetectionIOSim(VisionConstants constants) {
+    cameraName = constants.cameraName();
+    camera = new PhotonCamera(cameraName);
+    cameraMatrix = constants.intrinsicsMatrix();
+    distCoeffs = constants.distCoeffs();
+    this.constants = constants;
+  }
+
+  @Override
+  public void updateInputs(NoteDetectionIOInputs inputs) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'updateInputs'");
+  }
+
+  @Override
+  public String getName() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getName'");
+  }
+}

src/main/java/frc/robot/subsystems/vision/Vision.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.numbers.N5;
+import frc.robot.utils.vision.VisionIOInputsLogged;
 import java.util.Optional;
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;

src/main/java/frc/robot/subsystems/vision/VisionHelper.java

@@ -8,14 +8,19 @@
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import frc.robot.Robot;
@@ -27,6 +32,7 @@
 import org.littletonrobotics.junction.LogTable;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonUtils;
 import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.simulation.VisionSystemSim;
@@ -437,4 +443,30 @@ public static Field2d getSimDebugField(VisionSystemSim visionSim) {
     if (!Robot.isSimulation()) return null;
     return visionSim.getDebugField();
   }
+  //6995
+  public static double getDistance(PhotonTrackedTarget target) {
+        return PhotonUtils.calculateDistanceToTargetMeters(
+            0, 0, 0, Units.degreesToRadians(target.getPitch())); // TODO fix
+    }
+public static Translation2d getNoteTargetOffset(PhotonTrackedTarget target, double cameraX) {
+  return new Translation2d(getDistance(target), Rotation2d.fromDegrees(-target.getYaw()))
+.plus(new Translation2d(cameraX, 0));
+}
+
+public static List<Pose2d> getTargets(Pose2d robotPose, PhotonPipelineResult result, double cameraX) {
+    if (result == null) {
+        return List.of();
+    }
+    // TODO add sim logic
+    if (!result.hasTargets()) return List.of();
+    var list = result.getTargets();
+    list.removeIf((t)-> (t.getPitch() > 9));
+    return list.stream().map((t)->robotPose.transformBy(new Transform2d(getNoteTargetOffset(t, cameraX), Rotation2d.fromDegrees(-t.getYaw())))).toList();
+}
+
+public static Optional<Pose2d> getBestTarget(Pose2d robotPose, PhotonPipelineResult result, double cameraX) {
+    var targets = getTargets(robotPose, result, cameraX);
+    if (targets.size() == 0) {return Optional.empty();}
+    return Optional.of(targets.get(0));
+}
 }

src/main/java/frc/robot/utils/vision/NoteDetectionIOInputsLogged.java

@@ -0,0 +1,46 @@
+package frc.robot.utils.vision;
+
+import frc.robot.subsystems.vision.NoteDetectionIO;
+import frc.robot.subsystems.vision.VisionHelper;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+public class NoteDetectionIOInputsLogged extends NoteDetectionIO.NoteDetectionIOInputs
+    implements LoggableInputs, Cloneable {
+  @Override
+  public void toLog(LogTable table) {
+    table.put("Timestamp", timestamp);
+    table.put("Latency", latency);
+    for (int i = 0; i < targets.size(); i++) {
+      VisionHelper.Logging.logPhotonTrackedTarget(targets.get(i), table, String.valueOf(i));
+    }
+    table.put("NumTargets", targets.size());
+    VisionHelper.Logging.logVisionConstants(constants, table);
+    table.put("Note Yaw", yaw);
+    table.put("Note Pitch", pitch);
+    table.put("Note Distance", distance);
+  }
+
+  @Override
+  public void fromLog(LogTable table) {
+    timestamp = table.get("Timestamp", timestamp);
+    latency = table.get("Latency", latency);
+    for (int i = 0; i < table.get("NumTargets", numTargets); i++) {
+      this.targets.add(VisionHelper.Logging.getLoggedPhotonTrackedTarget(table, String.valueOf(i)));
+    }
+    constants = VisionHelper.Logging.getLoggedVisionConstants(table);
+    yaw = table.get("Note Yaw", yaw);
+    pitch = table.get("Note Pitch", pitch);
+    distance = table.get("Note Distance", distance);
+  }
+
+  public NoteDetectionIOInputsLogged clone() {
+    NoteDetectionIOInputsLogged copy = new NoteDetectionIOInputsLogged();
+    copy.timestamp = this.timestamp;
+    copy.latency = this.latency;
+    copy.yaw = this.yaw;
+    copy.pitch = this.pitch;
+    copy.distance = this.distance;
+    return copy;
+  }
+}

src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java → src/main/java/frc/robot/utils/vision/VisionIOInputsLogged.java

@@ -1,7 +1,9 @@
-package frc.robot.subsystems.vision;
+package frc.robot.utils.vision;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.subsystems.vision.VisionHelper;
+import frc.robot.subsystems.vision.VisionIO;
 import org.littletonrobotics.junction.LogTable;
 import org.littletonrobotics.junction.inputs.LoggableInputs;