Make SimpleNote serializable for AScope (#280)

* Make SimpleNote serializable for AScope

* Remove unintended import

---------

Co-authored-by: Alex Schokking <[email protected]>

src/main/java/competition/subsystems/vision/SimpleNote.java

@@ -7,6 +7,8 @@ public class SimpleNote implements StructSerializable {
     final double yaw;
     final double pitch;
 
+    public static final SimpleNoteStruct struct = new SimpleNoteStruct();
+
     public SimpleNote(double area, double yaw, double pitch) {
         this.area = area;
         this.yaw = yaw;
@@ -21,5 +23,7 @@ public double getYaw() {
         return yaw;
     }
 
-    public double getPitch() { return pitch; }
+    public double getPitch() {
+        return pitch;
+    }
 }

src/main/java/competition/subsystems/vision/SimpleNoteStruct.java

@@ -0,0 +1,42 @@
+package competition.subsystems.vision;
+
+import edu.wpi.first.util.struct.Struct;
+
+import java.nio.ByteBuffer;
+
+public class SimpleNoteStruct implements Struct<SimpleNote> {
+    @Override
+    public Class<SimpleNote> getTypeClass() {
+        return SimpleNote.class;
+    }
+
+    @Override
+    public String getTypeString() {
+        return "struct:SimpleNote";
+    }
+
+    @Override
+    public int getSize() {
+        return kSizeDouble * 3;
+    }
+
+    @Override
+    public String getSchema() {
+        return "double area; double yaw; double pitch";
+    }
+
+    @Override
+    public SimpleNote unpack(ByteBuffer byteBuffer) {
+        var area = byteBuffer.getDouble();
+        var yaw = byteBuffer.getDouble();
+        var pitch = byteBuffer.getDouble();
+        return new SimpleNote(area, yaw, pitch);
+    }
+
+    @Override
+    public void pack(ByteBuffer byteBuffer, SimpleNote simpleNote) {
+        byteBuffer.putDouble(simpleNote.getArea());
+        byteBuffer.putDouble(simpleNote.getYaw());
+        byteBuffer.putDouble(simpleNote.getPitch());
+    }
+}

src/main/java/competition/subsystems/vision/VisionSubsystem.java

@@ -396,10 +396,10 @@ public void periodic() {
         }, () -> {
             aKitLog.record("CenterCamLargestTargetArea", -1.0);
             aKitLog.record("CenterCamLargestTargetYaw", 0.0);
-            aKitLog.record("CenterCamLargestTargetPitch", 0);
+            aKitLog.record("CenterCamLargestTargetPitch", 0.0);
         });
 
-        // aKitLog.record("CenterlineDetections", centerlineDetections);
+        aKitLog.record("CenterlineDetections", centerlineDetections);
         aKitLog.record("DetectedNotes", detectedNotes);
         aKitLog.record("PassiveDetectedNotes", passiveDetectedNotes);
     }