stuff for multi note

KHS-Robotics · Jan 30, 2024 · ec0d404 · ec0d404
1 parent 43f855a
commit ec0d404
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -34,5 +34,6 @@ public class Constants {
 
   public static final Transform3d FRONT_APRILTAG_CAMERA_OFFSET = new Transform3d(0, 0, 0.43815, new Rotation3d(0, Math.toRadians(-23.6607), 0));
   public static final Transform3d REAR_APRILTAG_CAMERA_OFFSET = new Transform3d(0, 0, 0, new Rotation3d());
+  public static final double FRONT_NOTE_CAMERA_HFOV = 53;
 
 }
diff --git a/src/main/java/frc/robot/subsystems/cameras/Note.java b/src/main/java/frc/robot/subsystems/cameras/Note.java
@@ -3,14 +3,16 @@
 import java.util.ArrayList;
 import java.util.List;
 
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.estimator.KalmanFilter;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N1;
 
 public class Note {
   public Translation2d position;
   private List<Translation2d> pointCloud;
   private final double MAX_ERROR_METERS = 0.5;
 
-
   public Note(Translation2d initialPose) {
     this.position = initialPose;
     this.pointCloud = new ArrayList<>();

diff --git a/src/main/java/frc/robot/subsystems/cameras/NoteDetectorCamera.java b/src/main/java/frc/robot/subsystems/cameras/NoteDetectorCamera.java
@@ -84,24 +84,29 @@ private Note getNearestNote() {
   @Override
   public void periodic() {
     for (PhotonTrackedTarget t : getTargets()) {
-      // Translation2d notePose = estimateNotePose(t);
-
-      // if (notes.isEmpty()) {
-      //   notes.add(new Note(notePose));
-      // } else {
-      //   for (Note n : notes) {
-      //     if (!n.addPose(notePose)) {
-      //       notes.add(new Note(notePose));
-      //       break;
-      //     }
-      //   }
-      // }
       Translation2d notePose = estimateNotePose(t);
+
+      if (notes.isEmpty()) {
+        notes.add(new Note(notePose));
+      } else {
+        for (Note n : notes) {
+          if (!n.addPose(notePose)) {
+            notes.add(new Note(notePose));
+          }
+        }
+      }
+
+
+
       notes.get(0).addPose(notePose);
     }
 
-    RobotContainer.field.getObject("note").setPose(new Pose2d(notes.get(0).position, new Rotation2d()));
-    //RobotContainer.field.getObject("note").setPose(new Pose2d(getNearestNote().position, new Rotation2d()));
+    List<Pose2d> allNotes = new ArrayList<>();
+    for (Note n : notes) {
+      allNotes.add(new Pose2d(n.position, new Rotation2d()));
+    }
+
+    RobotContainer.field.getObject("note").setPoses(allNotes);
   }