First tinkering iteration, missing some stuff

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

@@ -4,16 +4,44 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.constants.CompetitionConstants;
 
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
 
   private final RobotContainer m_robotContainer;
 
   public Robot() {
+    Logger.recordMetadata("ProjectName", "2026_Robot_Code");
+
+    if(isReal()) {
+      if(CompetitionConstants.logToNetworkTables) {
+        Logger.addDataReceiver(new NT4Publisher());
+      }
+      
+      Logger.addDataReceiver(new WPILOGWriter());
+
+      new PowerDistribution(1, ModuleType.kRev);
+    } else {
+      setUseTiming(false);
+      String logPath = LogFileUtil.findReplayLog();
+      Logger.setReplaySource(new WPILOGReader(logPath));
+      Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+    }
+
+    Logger.start();
+    
     m_robotContainer = new RobotContainer();
   }
 

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

@@ -6,13 +6,33 @@ package frc.robot;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.constants.OIConstants;
+import frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer {
+  private Drivetrain drivetrain;
+
+  private CommandXboxController driver;
+
   public RobotContainer() {
+    drivetrain = new Drivetrain();
+
+    driver = new CommandXboxController(OIConstants.kDriverControllerPort);
+
     configureBindings();
   }
 
-  private void configureBindings() {}
+  private void configureBindings() {
+    drivetrain.setDefaultCommand(
+      drivetrain.drive(
+        driver::getLeftX, 
+        driver::getLeftY, 
+        driver::getRightX, 
+        () -> true
+      )
+    );
+  }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");

src/main/java/frc/robot/constants/CompetitionConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class CompetitionConstants {
+    // THIS SHOULD BE FALSE DURING COMPETITION PLAY
+    public static final boolean logToNetworkTables = true;
+}

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -0,0 +1,62 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+
+public class DrivetrainConstants {
+    // TODO Hold over from 2025, adjust?
+    public static final double kMaxSpeedMetersPerSecond = 4.125;
+    public static final double kMaxAngularSpeed = 2 * Math.PI;
+
+    // TODO Replace zeros with real numbers
+    public static final double kTrackWidth = Units.inchesToMeters(0);
+    public static final double kWheelBase = Units.inchesToMeters(0);
+
+    // TODO Replace zeros with real numbers
+    // These values should be determinable by writing the magnetic encoder output
+    // to the dashboard, and manually aligning all wheels such that the bevel gears
+    // on the side of the wheel all face left. Some known straight edge (like 1x1 or similar)
+    // should be used as the alignment medium. If done correctly, and the modules aren't disassembled,
+    // then these values should work throughout the season the first time they are set. 
+    public static final double kFrontLeftMagEncoderOffset = 0;
+    public static final double kFrontRightMagEncoderOffset = 0;
+    public static final double kRearLeftMagEncoderOffset = 0;
+    public static final double kRearRightMagEncoderOffset = 0;
+
+    // Kraken CAN IDs 
+    // TODO Real CAN IDs
+    public static final int kFrontLeftDrivingCANID = 0;
+    public static final int kFrontRightDrivingCANID = 0;
+    public static final int kRearLeftDrivingCANID = 0;
+    public static final int kRearRightDrivingCANID = 0;
+
+    // SparkMAX CAN IDs
+    // TODO Real CAN IDs
+    public static final int kFrontLeftTurningCANID = 0;
+    public static final int kFrontRightTurningCANID = 0;
+    public static final int kRearLeftTurningCANID = 0;
+    public static final int kRearRightTurningCANID = 0;
+
+    // Analog Encoder Input Ports 
+    // TODO Real Port IDs
+    public static final int kFrontLeftAnalogInPort = 0;
+    public static final int kFrontRightAnalogInPort = 0;
+    public static final int kRearLeftAnalogInPort = 0;
+    public static final int kRearRightAnalogInPort = 0;
+
+    public static final boolean kGyroReversed = true;
+
+    // TODO Hold over from 2025, adjust?
+    public static final double kHeadingP = .1;
+    public static final double kXTranslationP = .5;
+    public static final double kYTranslationP = .5;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
+    public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+        new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)
+    );
+}

src/main/java/frc/robot/constants/KrakenMotorConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class KrakenMotorConstants {
+    public static final double kFreeSpeedRPM = 6000;
+}

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -0,0 +1,102 @@
+package frc.robot.constants;
+
+import com.ctre.phoenix6.configs.AudioConfigs;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.math.util.Units;
+
+public class ModuleConstants {
+    // DRIVING MOTOR CONFIG (Kraken)
+    // TODO Replace with something other than 0
+    public static final double kDrivingMotorReduction = 0;
+
+    public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60;
+    public static final double kWheelDiameterMeters = Units.inchesToMeters(4);
+    public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
+    public static final double kDriveWheelFreeSpeedRPS = (kDrivingMotorFeedSpeedRPS * kWheelCircumferenceMeters) /
+        kDrivingMotorReduction;
+    public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRPS;
+    
+    // TODO Hold over from 2025, adjust?
+    public static final double kDriveP = .04;
+    public static final double kDriveI = 0;
+    public static final double kDriveD = 0;
+    public static final double kDriveS = 0;
+    public static final double kDriveV = kDrivingVelocityFeedForward;
+    public static final double kDriveA = 0;
+
+    // TODO Hold over from 2025, adjust?
+    public static final int kDriveMotorStatorCurrentLimit = 100;
+    public static final int kDriveMotorSupplyCurrentLimit = 65;
+
+    // TODO Hold over from 2025, adjust?
+    public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
+
+    // TURNING MOTOR CONFIG (NEO)
+
+    // TODO Hold over from 2025, adjust?
+    public static final double kTurningMotorReduction = 0;
+    public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
+    public static final double kTurnP = 1;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0;
+    
+    public static final int kTurnMotorCurrentLimit = 20;
+
+    public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
+
+    public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
+    public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
+    public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
+    public static final AudioConfigs kAudioConfig = new AudioConfigs();
+    public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
+
+    static {
+        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
+
+        kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
+        kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true;
+        kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
+        kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit;
+
+        kDriveMotorConfig.Inverted = kDriveInversionState;
+        kDriveMotorConfig.NeutralMode = kDriveIdleMode;
+
+        kAudioConfig.AllowMusicDurDisable = true;
+
+        kDriveSlot0Config.kP = kDriveP;
+        kDriveSlot0Config.kI = kDriveI;
+        kDriveSlot0Config.kD = kDriveD;
+        kDriveSlot0Config.kS = kDriveS;
+        kDriveSlot0Config.kV = kDriveV;
+        kDriveSlot0Config.kA = kDriveA;
+
+        turningConfig
+            .idleMode(kTurnIdleMode)
+            .smartCurrentLimit(kTurnMotorCurrentLimit);
+        turningConfig.encoder
+            .inverted(true)
+            .positionConversionFactor(kTurningFactor)
+            .velocityConversionFactor(kTurningFactor / 60.0);
+        turningConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kTurnP, kTurnI, kTurnD)
+            .outputRange(-1, 1)
+            .positionWrappingEnabled(true)
+            .positionWrappingInputRange(0, 2 * Math.PI);
+    }
+}

src/main/java/frc/robot/interfaces/IAprilTagProvider.java

@@ -0,0 +1,40 @@
+package frc.robot.interfaces;
+
+import java.util.OptionalDouble;
+
+/**
+ * An interface which ensures a class can provide common AprilTag oriented
+ * information from various sources in a consistent way.
+ */
+public interface IAprilTagProvider {
+    /**
+     * A method to get the tags currently in the camera's field of view
+     * @return
+     */
+    public int[] getVisibleTagIDs();
+
+    /**
+     * A method to get the distance from <i>the camera</i> to the AprilTag specified
+     * 
+     * @param id The ID of the AprilTag to give a distance to
+     * @param targetHeightMeters The height of the AprilTag off the ground, in meters
+     * @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view
+     */
+    public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
+
+    /**
+     * A method to get the pitch from the center of the image of a particular AprilTag
+     * 
+     * @param id The ID of the AprilTag to get the pitch of
+     * @return The pitch, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
+     */
+    public OptionalDouble getTagPitchByID(int id);
+
+    /**
+     * A method to get the yaw from the center of the image of a particular AprilTag
+     * 
+     * @param id The ID of the AprilTag to get the yaw of
+     * @return The yaw, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
+     */
+    public OptionalDouble getTagYawByID(int id);
+}

src/main/java/frc/robot/interfaces/IVisualPoseProvider.java

@@ -0,0 +1,27 @@
+package frc.robot.interfaces;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/**
+ * An interface which ensures a class' ability to provide visual pose information
+ * in a consistent way
+ */
+public interface IVisualPoseProvider {
+    /**
+     * A record that can contain the two elements necessary for a WPILIB
+     * pose estimator to use the information from a vision system as part of a full
+     * robot pose estimation
+     */
+    public record VisualPose(Pose2d visualPose, double timestamp) {}
+
+    /**
+     * Return a VisualPose or null if an empty Optional if none is available.
+     * Implementation should provide an empty response if it's unable to provide
+     * a reliable pose, or any pose at all.
+     * 
+     * @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided
+     */
+    public Optional<VisualPose> getVisualPose();
+}

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

@@ -0,0 +1,194 @@
+package frc.robot.subsystems;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.DrivetrainConstants;
+import frc.robot.constants.OIConstants;
+import frc.robot.utilities.SwerveModule;
+
+public class Drivetrain extends SubsystemBase {
+    private SwerveModule frontLeft;
+    private SwerveModule frontRight;
+    private SwerveModule rearLeft;
+    private SwerveModule rearRight;
+
+    private AHRS gyro;
+
+    private SwerveDrivePoseEstimator estimator;
+
+    public Drivetrain() {
+        frontLeft = new SwerveModule(
+            "FrontLeft", 
+            DrivetrainConstants.kFrontLeftDrivingCANID, 
+            DrivetrainConstants.kFrontLeftTurningCANID, 
+            DrivetrainConstants.kFrontLeftAnalogInPort, 
+            DrivetrainConstants.kFrontLeftMagEncoderOffset
+        );
+
+        frontRight = new SwerveModule(
+            "FrontRight", 
+            DrivetrainConstants.kFrontRightDrivingCANID, 
+            DrivetrainConstants.kFrontRightTurningCANID, 
+            DrivetrainConstants.kFrontRightAnalogInPort, 
+            DrivetrainConstants.kFrontRightMagEncoderOffset
+        );
+
+        rearLeft = new SwerveModule(
+            "RearLeft", 
+            DrivetrainConstants.kRearLeftDrivingCANID, 
+            DrivetrainConstants.kRearLeftTurningCANID, 
+            DrivetrainConstants.kRearLeftAnalogInPort, 
+            DrivetrainConstants.kRearLeftMagEncoderOffset
+        );
+
+        rearRight = new SwerveModule(
+            "RearRight", 
+            DrivetrainConstants.kRearRightDrivingCANID, 
+            DrivetrainConstants.kRearRightTurningCANID, 
+            DrivetrainConstants.kRearRightAnalogInPort, 
+            DrivetrainConstants.kRearRightMagEncoderOffset
+        );
+
+        gyro = new AHRS(NavXComType.kMXP_SPI);
+
+        // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
+        estimator = new SwerveDrivePoseEstimator(
+            DrivetrainConstants.kDriveKinematics, 
+            Rotation2d.fromDegrees(getGyroValue()), 
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            },
+            new Pose2d()
+        );
+    }
+
+    @Override
+    public void periodic() {
+        estimator.update(
+            Rotation2d.fromDegrees(getGyroValue()),
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            }
+        );
+
+        frontLeft.periodic();
+        frontRight.periodic();
+        rearLeft.periodic();
+        rearRight.periodic();
+
+        Logger.recordOutput("Drivetrain/Pose", getPose());
+        Logger.recordOutput("Drivetrain/Heading", getHeading());
+    }
+
+    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
+        // TODO Inversions? Specific Alliance code?
+        return run(() -> {
+            drive(
+                MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), 
+                MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), 
+                fieldRelative.getAsBoolean()
+            );
+        });
+    }
+
+    public Command setX() {
+        return run(() -> {
+            frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+            frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+            rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+            rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+        });
+    }
+
+    public void resetEncoders() {
+        frontLeft.resetEncoders();
+        frontRight.resetEncoders();
+        rearLeft.resetEncoders();
+        rearRight.resetEncoders();
+    }
+
+    public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
+        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
+        double xSpeedDelivered = 0;
+        double ySpeedDelivered = 0;
+
+        if(p != 0){
+            xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        }else{
+            xSpeedDelivered = 0;
+            ySpeedDelivered = 0;
+        }
+
+        double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
+
+        SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
+            fieldRelative ?
+                ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
+                    estimator.getEstimatedPosition().getRotation()) :
+                new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
+        );
+
+        setModuleStates(swerveModuleStates);
+    }
+
+    public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
+        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
+        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
+
+        setModuleStates(newStates);
+    }
+
+    public void setModuleStates(SwerveModuleState[] desiredStates) {
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            desiredStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+        frontLeft.setDesiredState(desiredStates[0]);
+        frontRight.setDesiredState(desiredStates[1]);
+        rearLeft.setDesiredState(desiredStates[2]);
+        rearRight.setDesiredState(desiredStates[3]);
+    }
+
+    public ChassisSpeeds getCurrentChassisSpeeds() {
+        return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
+            frontLeft.getState(),
+            frontRight.getState(),
+            rearLeft.getState(),
+            rearRight.getState()
+        );
+    }
+
+    public Pose2d getPose() {
+        return estimator.getEstimatedPosition();
+    }
+
+    public double getGyroValue() {
+        return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
+    }
+
+    public double getHeading() {
+        return estimator.getEstimatedPosition().getRotation().getDegrees();
+    }
+}

src/main/java/frc/robot/utilities/PhotonVision.java

@@ -0,0 +1,166 @@
+package frc.robot.utilities;
+
+import java.io.IOException;
+import java.util.List;
+import java.util.Optional;
+import java.util.OptionalDouble;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonUtils;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.interfaces.IAprilTagProvider;
+import frc.robot.interfaces.IVisualPoseProvider;
+
+public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
+
+    private final PhotonCamera camera;
+
+    private final PhotonPoseEstimator photonPoseEstimator;
+
+    private final double cameraHeightMeters;
+    private final double cameraPitchRadians;
+
+    private PhotonPipelineResult latestResult;
+
+    public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
+        camera = new PhotonCamera(cameraName);
+
+        photonPoseEstimator = new PhotonPoseEstimator(
+            AprilTagFieldLayout.loadFromResource(
+                AprilTagFields.kDefaultField.m_resourceFile
+            ), 
+            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
+            robotToCam
+        );
+
+        this.cameraHeightMeters = cameraHeightMeters;
+        this.cameraPitchRadians = cameraPitchRadians;
+
+        this.latestResult = null;
+    }
+
+    public void periodic() {
+        // TODO Do we care about missed results? Probably not, if we're taking long enough to miss results something else is wrong
+        List<PhotonPipelineResult> results = camera.getAllUnreadResults();
+
+        if(!results.isEmpty()) {
+            latestResult = results.get(results.size() - 1);
+        }
+    }
+
+    @Override
+    public Optional<VisualPose> getVisualPose() {
+        if(latestResult == null) {
+            return Optional.empty();
+        }
+
+        Optional<EstimatedRobotPose> pose = photonPoseEstimator.update(latestResult);
+
+        if (pose.isEmpty()) {
+            return Optional.empty();
+        }
+
+        return Optional.of(
+            new VisualPose(
+                pose.get().estimatedPose.toPose2d(), 
+                pose.get().timestampSeconds
+            )
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
+        if (latestResult == null) {
+            return OptionalDouble.empty();
+        }
+
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            PhotonUtils.calculateDistanceToTargetMeters(
+                cameraHeightMeters, 
+                targetHeightMeters, 
+                cameraPitchRadians, 
+                Units.degreesToRadians(desiredTarget.get().getPitch()))
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagPitchByID(int id) {
+        if(latestResult == null) {
+            OptionalDouble.empty();
+        }
+        
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            desiredTarget.get().getPitch()
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagYawByID(int id) {
+        if(latestResult == null) {
+            OptionalDouble.empty();
+        }
+        
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            desiredTarget.get().getYaw()
+        );
+    }
+
+    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
+        for (PhotonTrackedTarget target : targets) {
+            if (target.getFiducialId() == id) {
+                return Optional.of(target);
+            }
+        }
+
+        return Optional.empty();
+    }
+
+    @Override
+    public int[] getVisibleTagIDs() {
+        if(latestResult == null) {
+            return new int[] {};
+        }
+
+        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
+    }
+    
+}

src/main/java/frc/robot/utilities/SwerveModule.java

@@ -0,0 +1,123 @@
+package frc.robot.utiltiies;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import frc.robot.constants.ModuleConstants;
+
+/*
+ * This thread
+ * 
+ * https://www.chiefdelphi.com/t/best-easiest-way-to-connect-wire-and-program-thrifty-absolute-magnetic-encoder-to-rev-spark-max-motor-controller/439040/30
+ * 
+ * implies that the best use of the thrifty absolute encoder is to use it as a reference for the Spark relative encoder and then
+ * used the closed loop control on the controller for turning
+ * 
+ * IDK if that's really necessary, the read rate of the analog ports is 100HZ, I suppose the only benefit is the higher rate of
+ * the controller closed loop controller. 
+ */
+public class SwerveModule {
+    private TalonFX drive;
+    private SparkMax turning;
+
+    private RelativeEncoder turningRelativeEncoder;
+
+    private AnalogEncoder turningAbsoluteEncoder;
+
+    private SparkClosedLoopController turningClosedLoopController;
+
+    private VelocityVoltage driveVelocityRequest;
+
+    private String moduleName;
+
+    public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
+        drive = new TalonFX(drivingCANID);
+        turning = new SparkMax(turningCANID, MotorType.kBrushless);
+
+        turningRelativeEncoder = turning.getEncoder();
+
+        turningAbsoluteEncoder = new AnalogEncoder(analogEncoderID, 2 * Math.PI, analogEncoderOffset);
+
+        turningClosedLoopController = turning.getClosedLoopController();
+
+        drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+        drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+
+        turning.configure(
+            ModuleConstants.turningConfig,
+            ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters
+        );
+
+        turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        drive.setPosition(0);
+
+        this.moduleName = "Drivetrain/Modules/" + moduleName;
+    }
+
+    public void periodic() {
+        Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+        Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
+        Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
+    }
+
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(
+            drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
+            new Rotation2d(turningRelativeEncoder.getPosition())
+        );
+    }
+
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(
+            drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
+            new Rotation2d(turningRelativeEncoder.getPosition())
+        );
+    }
+
+    public void setDesiredState(SwerveModuleState desiredState) {
+        // TODO is this really necessary, the offset is managed by the Absolute Encoder
+        // and its "source of truth" behavior in relation to the relative encoder
+        // Probably doesn't *hurt* that it's here, but it may not be needed
+        desiredState.optimize(new Rotation2d(turningRelativeEncoder.getPosition()));
+
+        drive.setControl(
+            driveVelocityRequest.withVelocity(
+                desiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
+            ).withFeedForward(
+                desiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
+            )
+        );
+
+        turningClosedLoopController.setSetpoint(
+            desiredState.angle.getRadians(), 
+            ControlType.kPosition
+        );
+    }
+
+    public void resetEncoders() {
+        drive.setPosition(0);
+
+        zeroTurningEncoder();
+    }
+
+    public void zeroTurningEncoder() {
+        turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+    }
+}