April Tag and Constants

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

@@ -15,6 +15,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.constants.Constants;
 import frc.robot.utils.logging.commands.CommandLogger;
 
 /**

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

@@ -11,6 +11,7 @@
 import frc.robot.commands.roller.SpinRoller;
 import frc.robot.commands.tilt.TiltDown;
 import frc.robot.commands.tilt.TiltUp;
+import frc.robot.constants.Constants;
 import frc.robot.subsystems.RollerSubsystem;
 import frc.robot.subsystems.TiltSubsystem;
 import frc.robot.utils.simulation.RobotVisualizer;

src/main/java/frc/robot/apriltags/ApriltagIO.java

@@ -0,0 +1,5 @@
+package frc.robot.apriltags;
+
+import frc.robot.utils.logging.io.BaseIo;
+
+public interface ApriltagIO extends BaseIo {}

src/main/java/frc/robot/apriltags/ApriltagInputs.java

@@ -0,0 +1,38 @@
+package frc.robot.apriltags;
+
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+import org.littletonrobotics.junction.LogTable;
+
+public class ApriltagInputs implements LoggableInputs {
+  public double[] timestamp = new double[0];
+  public double[] serverTime = new double[0];
+  public double[] posX = new double[0];
+  public double[] posY = new double[0];
+  public double[] poseYaw = new double[0];
+  public double[] distanceToTag = new double[0];
+  public int[] apriltagNumber = new int[0];
+
+
+
+  @Override
+  public void toLog(LogTable table) {
+    table.put("timestamp", timestamp);
+    table.put("serverTime", serverTime);
+    table.put("posX", posX);
+    table.put("posY", posY);
+    table.put("poseYaw", poseYaw);
+    table.put("distanceToTag", distanceToTag);
+    table.put("apriltagNumber", apriltagNumber);
+  }
+
+  @Override
+  public void fromLog(LogTable table) {
+    timestamp = table.get("timestamp", timestamp);
+    serverTime = table.get("serverTime", serverTime);
+    posX = table.get("posX", posX);
+    posY = table.get("posY", posY);
+    poseYaw = table.get("poseYaw", poseYaw);
+    distanceToTag = table.get("distanceToTag", distanceToTag);
+    apriltagNumber = table.get("apriltagNumber", apriltagNumber);
+  }
+}

src/main/java/frc/robot/apriltags/ApriltagReading.java

@@ -0,0 +1,10 @@
+package frc.robot.apriltags;
+
+public record ApriltagReading(
+    double posX,
+    double posY,
+    double poseYaw,
+    double distanceToTag,
+    int apriltagNumber,
+    double latency,
+    double measurementTime) {}

src/main/java/frc/robot/apriltags/MockApriltag.java

@@ -0,0 +1,11 @@
+package frc.robot.apriltags;
+
+import frc.robot.utils.logging.io.BaseIoImpl;
+
+public class MockApriltag extends BaseIoImpl<ApriltagInputs> implements ApriltagIO {
+  public MockApriltag(String name, ApriltagInputs inputs) {
+          super(name, inputs);
+      }
+  @Override
+  protected void updateInputs(ApriltagInputs inputs) {}
+}

src/main/java/frc/robot/apriltags/TCPApriltag.java

@@ -0,0 +1,57 @@
+package frc.robot.apriltags;
+
+import edu.wpi.first.math.geometry.*;
+import frc.robot.constants.Constants;
+import frc.robot.utils.Apriltag;
+import frc.robot.utils.logging.io.BaseIoImpl;
+import java.util.Queue;
+import org.littletonrobotics.junction.Logger;
+
+public class TCPApriltag extends BaseIoImpl<ApriltagInputs> implements ApriltagIO{
+  private final TCPApriltagServer server;
+
+  public TCPApriltag(String name, ApriltagInputs inputs) {
+    super(name, inputs);
+    server = new TCPApriltagServer(Constants.TCP_SERVER_PORT);
+    server.start();
+  }
+
+  @Override
+  protected void updateInputs(ApriltagInputs inputs) {
+    Queue<ApriltagReading> queue = server.flush();
+    int queueSize = queue.size();
+    Logger.recordOutput("VisionMeasurementsThisTick", queueSize);
+    inputs.posX = new double[queueSize];
+    inputs.posY = new double[queueSize];
+    inputs.poseYaw = new double[queueSize];
+    inputs.distanceToTag = new double[queueSize];
+    inputs.apriltagNumber = new int[queueSize];
+    inputs.serverTime = new double[queueSize];
+    inputs.timestamp = new double[queueSize];
+
+    Translation3d[] apriltagPoseArray = new Translation3d[queueSize];
+    Pose2d[] visionPoseArray = new Pose2d[queueSize];
+
+    for (int i = 0; i < queueSize; i++) {
+      ApriltagReading measurement = queue.poll();
+      inputs.posX[i] = measurement.posX();
+      inputs.posY[i] = measurement.posY();
+      inputs.poseYaw[i] = measurement.poseYaw();
+      inputs.distanceToTag[i] = measurement.distanceToTag();
+      inputs.apriltagNumber[i] = measurement.apriltagNumber();
+      inputs.timestamp[i] = measurement.latency();
+      inputs.serverTime[i] = measurement.measurementTime();
+
+      Apriltag apriltag = Apriltag.of(measurement.apriltagNumber());
+      apriltagPoseArray[i] =
+          apriltag == null ? new Translation3d(0, 0, 0) : apriltag.getTranslation();
+      visionPoseArray[i] =
+          new Pose2d(
+              measurement.posX(),
+              measurement.posY(),
+              Rotation2d.fromDegrees(measurement.poseYaw()));
+    }
+    Logger.recordOutput("Apriltag/TagPoses", apriltagPoseArray);
+    Logger.recordOutput("Apriltag/VisionPoses", visionPoseArray);
+  }
+}

src/main/java/frc/robot/apriltags/TCPApriltagServer.java

@@ -0,0 +1,42 @@
+package frc.robot.apriltags;
+
+import edu.wpi.first.wpilibj.Timer;
+import java.io.DataInputStream;
+import java.io.IOException;
+
+public class TCPApriltagServer extends TCPServer<ApriltagReading> {
+
+  public TCPApriltagServer(int port) {
+    super(port);
+  }
+
+  /**
+   * Format of message: [(double)x, (double)y, (double)yaw, (double)distance,(double)timestamp,
+   * (int)apriltagNumber]
+   */
+  @Override
+  protected ApriltagReading extractFromStream(DataInputStream stream) throws IOException {
+    double posX = -1;
+    double posY = -1;
+    double poseYaw = -1;
+    double distanceToTag = -1;
+    double timestamp = -1;
+    int apriltagNumber = -1;
+    double now = 0;
+    while (posX == -1
+        && posY == -1
+        && poseYaw == -1
+        && distanceToTag == -1
+        && apriltagNumber == -1
+        && timestamp == -1) {
+      posX = stream.readDouble();
+      posY = stream.readDouble();
+      poseYaw = stream.readDouble();
+      distanceToTag = stream.readDouble();
+      timestamp = stream.readDouble();
+      apriltagNumber = stream.readInt();
+      now = Timer.getFPGATimestamp() * 1000;
+    }
+    return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, timestamp, now);
+  }
+}

src/main/java/frc/robot/apriltags/TCPServer.java

@@ -0,0 +1,120 @@
+package frc.robot.apriltags;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.constants.Constants;
+import frc.robot.utils.logging.commands.CommandLogger;
+import java.io.BufferedInputStream;
+import java.io.DataInputStream;
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.LinkedList;
+import java.util.Queue;
+import java.util.concurrent.LinkedTransferQueue;
+
+public abstract class TCPServer<T> extends Thread {
+
+  private final int port;
+  private ServerSocket serverSocket;
+  private Socket clientSocket;
+  private DataInputStream dataInputStream;
+  private BufferedInputStream bufferedInputStream;
+  private boolean running = false;
+  private final LinkedTransferQueue<T> readings = new LinkedTransferQueue<>();
+
+  /**
+   * @param port for network communication. Port must match between client and server (range
+   *     5800-5810 (rio limitations))
+   */
+  public TCPServer(int port) {
+    this.port = port;
+  }
+
+  @Override
+  public void run() {
+    try {
+      this.serverSocket = new ServerSocket(port);
+      this.serverSocket.setSoTimeout(Constants.SERVER_SOCKET_CONNECTION_TIMEOUT);
+      this.clientSocket = null;
+    } catch (IOException e2) {
+      DriverStation.reportError("Could not start server on port " + port, true);
+      this.serverSocket = null;
+    }
+    running = true;
+    while (running) {
+      if (serverSocket == null) {
+        escape();
+        continue;
+      }
+      if (this.clientSocket == null) {
+        clientSocket = waitForClient();
+        if (clientSocket == null) {
+          continue;
+        }
+        try {
+          bufferedInputStream = new BufferedInputStream(clientSocket.getInputStream());
+          dataInputStream = new DataInputStream(bufferedInputStream);
+        } catch (IOException e) {
+          DriverStation.reportError("Could not fetch client Input Stream", true);
+          dataInputStream = null;
+          continue;
+        }
+      }
+      try {
+        readings.add(extractFromStream(dataInputStream));
+      } catch (IOException e) {
+        DriverStation.reportError("Problem while reading tcp stream data", true);
+        clientSocket = null;
+        bufferedInputStream = null;
+        dataInputStream = null;
+      }
+    }
+  }
+
+  protected abstract T extractFromStream(DataInputStream stream) throws IOException;
+
+  private Socket waitForClient() {
+    Socket s;
+    try {
+      s = serverSocket.accept();
+      CommandLogger.get().logMessage("TCP Client Connection Status", true);
+    } catch (IOException e1) {
+      CommandLogger.get().logMessage("TCP Client Connection Status", false);
+      s = null;
+    }
+    try {
+      Thread.sleep(Constants.SERVER_SOCKET_ATTEMPT_DELAY);
+    } catch (InterruptedException e) {
+      DriverStation.reportError("TCP Server thread interrupted: " + e.getMessage(), true);
+    }
+    return s;
+  }
+
+  public void escape() {
+    this.running = false;
+    try {
+      if (bufferedInputStream != null) {
+        bufferedInputStream.close();
+      }
+      if (dataInputStream != null) {
+        dataInputStream.close();
+      }
+      if (clientSocket != null) {
+        clientSocket.close();
+      }
+      if (serverSocket != null) {
+        serverSocket.close();
+      }
+    } catch (IOException e) {
+      DriverStation.reportError("Could not release thread resources!", true);
+    } finally {
+      CommandLogger.get().logMessage("TCP Client Connection Status", false);
+    }
+  }
+
+  public Queue<T> flush() {
+    Queue<T> queue = new LinkedList<>();
+    readings.drainTo(queue);
+    return queue;
+  }
+}

src/main/java/frc/robot/commands/roller/SpinRoller.java

@@ -1,7 +1,7 @@
 package frc.robot.commands.roller;
 
 import edu.wpi.first.wpilibj.Timer;
-import frc.robot.Constants;
+import frc.robot.constants.Constants;
 import frc.robot.subsystems.RollerSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 

src/main/java/frc/robot/commands/tilt/TiltDown.java

@@ -1,7 +1,7 @@
 package frc.robot.commands.tilt;
 //added a comment
 import edu.wpi.first.wpilibj.Timer;
-import frc.robot.Constants;
+import frc.robot.constants.Constants;
 import frc.robot.subsystems.TiltSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 // The command makes thing tilt down

src/main/java/frc/robot/commands/tilt/TiltUp.java

@@ -1,7 +1,7 @@
 package frc.robot.commands.tilt;
 
 import edu.wpi.first.wpilibj.Timer;
-import frc.robot.Constants;
+import frc.robot.constants.Constants;
 import frc.robot.subsystems.TiltSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 // This command makes thing tilt up

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

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.constants;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.RobotBase;
@@ -15,7 +15,7 @@
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
  */
-public final class Constants {
+public final class Constants extends Constants2025{
 
     public enum Mode {
         /**