first commit

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

@@ -0,0 +1,55 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class ClimberSubsystem extends SubsystemBase{
+    private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
+    private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT);
+
+    public void liftRobot() {
+        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED);
+    }
+
+    public void lowerRobot() {
+        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
+    }
+
+    public void stopClimber() {
+        climberMotor.set(0);
+    }
+
+    public Command liftRobotCommand() {
+        return runOnce(() -> toggleRatchet(true)).andThen(() -> liftRobot());
+    }
+
+    public Command lowerRobotCommand() {
+        return runOnce(() -> toggleRatchet(false)).andThen(() -> lowerRobot());
+    }
+
+    public Command stopClimberCommand() {
+        return runOnce(() -> stopClimber());
+    }
+
+    public static void toggleRatchet(boolean toggle) {
+    if (toggle == true) {
+      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
+    } else
+      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
+  }
+
+  public Command toggleRatchetCommand(boolean toggle) {
+    return runOnce(() -> toggleRatchet(toggle));
+  }
+
+  @Override
+  public void periodic()
+  {
+    SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition());
+  }
+}

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

@@ -0,0 +1,94 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.Constants;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+
+public class IntakeSubsystem extends SubsystemBase {
+
+    private static SparkFlex intakeMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
+            MotorType.kBrushless);
+    private static SparkClosedLoopController intakeRotatorPIDController;
+    public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
+
+    public IntakeSubsystem() {
+        intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D);
+        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+                com.revrobotics.PersistMode.kNoPersistParameters);
+        intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
+    }
+
+    public void startIntakeMotor() {
+        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED);
+    }
+
+    public void reverseIntakeMotor() {
+        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED * -1);
+    }
+
+    public void stopIntakeMotor() {
+        intakeMotor.set(0);
+    }
+
+    public Command startIntakeMotorCommand() {
+        return runOnce(() -> startIntakeMotor());
+    }
+
+    public Command reverseIntakeMotorCommand() {
+        return runOnce(() -> reverseIntakeMotor());
+    }
+
+    public Command stopIntakeMotorCommand() {
+        return runOnce(() -> stopIntakeMotor());
+    }
+
+    public void deployIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
+                ControlType.kPosition);
+    }
+
+    public Command deployintakeCommand() {
+        return runOnce(() -> deployIntake());
+    }
+
+    public void retractIntake() {
+        intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition);
+    }
+
+    public Command retractIntakeCommand() {
+        return runOnce(() -> retractIntake());
+    }
+
+    public void assistFuelIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
+                ControlType.kPosition);
+    }
+
+    public Command assistFuelIntakeCommand() {
+        return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
+                .andThen(new WaitCommand(2));
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
+    }
+}

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

@@ -0,0 +1,124 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+import java.util.function.BooleanSupplier;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.LimelightHelpers;
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+
+public class ShooterSubsystem extends SubsystemBase {
+    private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex indexerMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
+           // MotorType.kBrushless);
+
+    //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
+            //MotorType.kBrushless);
+
+    //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0);
+    //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0);
+
+    private static double currentPotentiometerPosition; // might need second value for the right potentiometer
+
+    public void startShooterMotors() {
+        centerShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        leftShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        rightShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+    }
+
+    public double getShooterMotorVelocity() {
+        return leftShooterMotor.getEncoder().getVelocity();
+    }
+
+    public void startIndexerMotor() {
+        // if (LimelightHelpers.getTX("limelight") < 1.5 &&
+        // LimelightHelpers.getTX("limelight") > -1.5) {
+        indexerMotor.set(Constants.ShooterConstants.INDEXER_MOTOR_SPEED);
+        // } else
+        // indexerMotor.set(0);
+    }
+
+    /* public Command shootFuelCommand() {
+        return run(() -> startShooterMotors())
+                .until(() -> {
+                    return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY);
+                })
+                .andThen(() -> startIndexerMotor());
+    } */
+
+    
+      public Command shootFuelCommand() {
+      return runOnce(() -> startShooterMotors()).andThen(new WaitCommand(2))
+      .andThen(() -> startIndexerMotor());
+      };
+     
+
+    public void stopShooters() {
+        centerShooterMotor.set(0);
+        leftShooterMotor.set(0);
+        rightShooterMotor.set(0);
+        indexerMotor.set(0);
+    }
+
+    public Command stopShooterCommand() {
+        return runOnce(() -> stopShooters());
+    }
+
+    public void moveActuator(double desiredPotentiometerPosition) {
+        if (desiredPotentiometerPosition > currentPotentiometerPosition) {
+            //TODO: Test for positive or negative power
+            //leftActuatorMotor.set(0.1);
+            //rightActuatorMotor.set(0.1);
+        } else {
+            //leftActuatorMotor.set(-0.1);
+            //rightActuatorMotor.set(-0.1);
+        }
+    }
+
+    public void stopActuator() {
+        //leftActuatorMotor.set(0);
+        //rightActuatorMotor.set(0);
+    }
+
+    public Command moveActuatorCommand(double desiredPotentiometerPosition) {
+        return run(() -> moveActuator(desiredPotentiometerPosition))
+                .until(() -> currentPotentiometerPosition == currentPotentiometerPosition)
+                .andThen(() -> stopActuator());
+    }
+
+    @Override
+    public void periodic() {
+        /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
+        SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
+        currentPotentiometerPosition = leftPotentiometer.get(); */
+    }
+}

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

@@ -0,0 +1,182 @@
+package frc.robot.subsystems;
+
+import java.util.List;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPoint;
+import com.pathplanner.lib.path.RotationTarget;
+import com.pathplanner.lib.path.Waypoint;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+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.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.Constants;
+import frc.robot.LimelightHelpers;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc.robot.RobotContainer;
+import frc.robot.Constants;
+
+public class TargetingSubsystems extends SubsystemBase {
+    
+    PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera");
+    Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
+    PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator(
+            AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
+            BACK_LEFT_CAMERA_OFFSETS);
+    PIDController photonAimPIDController = new PIDController(0.3, 0, 0.001);
+
+    public TargetingSubsystems() {
+        photonAimPIDController.enableContinuousInput(-180, 180);
+    }
+
+    Pose2d currentRobotPose;
+
+    public List<Waypoint> rightClimbWaypoints;
+
+    public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) {
+        GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
+        PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
+        currentRobotPose = swerveDrive.getPose();
+        rightClimbWaypoints = PathPlannerPath.waypointsFromPoses(
+                currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE);
+
+        PathPlannerPath goToClimbPath = new PathPlannerPath(rightClimbWaypoints, goToClimbConstraints, null,
+                goalEndState);
+        goToClimbPath.preventFlipping = true;
+
+        return swerveDrive.getAutonomousCommand("goToClimbPath");
+    }
+
+    public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
+        return new RunCommand(() -> {
+         currentRobotPose = swerveDrive.getPose();
+
+            Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
+
+            double xError = errorFromDesiredPose.getX();
+            double yError = errorFromDesiredPose.getY();
+            double angleError = errorFromDesiredPose.getRotation().getRadians();
+
+            PIDController xController = new PIDController(1.5, 0, 0);
+            PIDController yController = new PIDController(1.5, 0, 0);
+            PIDController angleController = new PIDController(3.0, 0, 0);
+
+            angleController.enableContinuousInput(-Math.PI, Math.PI);
+
+            double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
+            double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
+            double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(),
+                    desiredPose.getRotation().getRadians());
+            
+            swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
+        }, swerveDrive);
+    }
+
+    Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { 
+        return new RunCommand(() -> {
+        double rot = 0.0;
+        var result = photonVision.getLatestResult();
+        if (result.hasTargets()) {
+            double yawError = result.getBestTarget().getYaw();
+            rot = photonAimPIDController.calculate(yawError, 0);
+        }
+
+        rot = MathUtil.clamp(rot, -3.0, 3.0);
+
+        swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
+                driverXbox.getLeftX() * -1), rot, true);
+    }, swerveDrive);
+}
+
+
+    public PhotonPoseEstimator getPhotonPoseEstimator() {
+        return photonEstimator;
+    }
+
+    // static public NetworkTable table =
+    // NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
+    // static public NetworkTableEntry ty = table.getEntry("ty");
+    // static double targetOffsetAngle_Vertical = ty.getDouble(0.0);
+
+    // how many degrees back is your limelight rotated from perfectly vertical?
+    static double limelightMountAngleDegrees = 25.0;
+
+    // distance from the center of the Limelight lens to the floor
+    static double limelightLensHeightInches = 27.5;
+
+    // distance from the target to the floor
+    static double goalHeightInches = 44;
+
+    static double angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
+    static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
+
+    // calculate distance
+    static double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
+            / Math.tan(angleToGoalRadians);
+
+    public static double getDistanceFromAprilTag() {
+        angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
+        angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
+        distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
+                / Math.tan(angleToGoalRadians);
+        return distanceFromLimelightToGoalInches;
+    }
+
+    public void updateRobotPose(SwerveSubsystem swerveDrive) {
+         Optional<EstimatedRobotPose> result = photonEstimator.update(photonVision.getLatestResult());
+
+        if (result.isPresent()) {
+            EstimatedRobotPose estimatedPose = result.get();
+            swerveDrive.getSwerveDrive()
+                    .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
+        }
+    }
+    @Override
+    public void periodic() {
+
+        /*
+         * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
+         * photonVision.getLatestResult().getBestTarget().getYaw());
+         * Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value",
+         * photonVision.getLatestResult().getBestTarget().getPitch());
+         * Shuffleboard.getTab("Vision").add("Limelight TX Value",
+         * LimelightHelpers.getTX("limelight"));
+         * Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
+         * LimelightHelpers.getFiducialID("limelight"));
+         * Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null);
+         * Shuffleboard.getTab("Vision").addCamera("Photon",
+         * "Arducam_OV9281_USB_Camera",
+         * "http://photonvision.local:5800");
+         */
+    } 
+}
+

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

@@ -0,0 +1,720 @@
+// 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.swervedrive;
+
+import static edu.wpi.first.units.Units.Meter;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.commands.PathfindingCommand;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.DriveFeedforwards;
+import com.pathplanner.lib.util.swerve.SwerveSetpoint;
+import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import frc.robot.Constants;
+import frc.robot.subsystems.swervedrive.Vision.Cameras;
+import java.io.File;
+import java.io.IOException;
+import java.util.Arrays;
+import java.util.Optional;
+import java.util.concurrent.atomic.AtomicReference;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+import org.json.simple.parser.ParseException;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.targeting.PhotonPipelineResult;
+import swervelib.SwerveController;
+import swervelib.SwerveDrive;
+import swervelib.SwerveDriveTest;
+import swervelib.math.SwerveMath;
+import swervelib.parser.SwerveControllerConfiguration;
+import swervelib.parser.SwerveDriveConfiguration;
+import swervelib.parser.SwerveParser;
+import swervelib.telemetry.SwerveDriveTelemetry;
+import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
+
+public class SwerveSubsystem extends SubsystemBase {
+    /**
+     * Swerve drive object.
+     */
+    private final SwerveDrive swerveDrive;
+
+    /**
+     * Enable vision odometry updates while driving.
+     */
+    private final boolean visionDriveTest = false;
+
+    /**
+     * PhotonVision class to keep an accurate odometry.
+     */
+    private Vision vision;
+
+    /**
+     * Initialize {@link SwerveDrive} with the directory provided.
+     *
+     * @param directory Directory of swerve drive config files.
+     */
+    public SwerveSubsystem(File directory) {
+        boolean blueAlliance = false;
+        Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1),
+                Meter.of(4)),
+                Rotation2d.fromDegrees(0))
+                : new Pose2d(new Translation2d(Meter.of(16),
+                        Meter.of(4)),
+                        Rotation2d.fromDegrees(180));
+        // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary
+        // objects being created.
+        SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
+        try {
+            swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose);
+            // Alternative method if you don't want to supply the conversion factor via JSON
+            // files.
+            // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed,
+            // angleConversionFactor, driveConversionFactor);
+        } catch (Exception e) {
+            throw new RuntimeException(e);
+        }
+        swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot
+                                                 // via
+                                                 // angle.
+        swerveDrive.setCosineCompensator(false);// !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation
+                                                // for
+                                                // simulations since it causes discrepancies not seen in real life.
+        swerveDrive.setAngularVelocityCompensation(true,
+                true,
+                0.1); // Correct for skew that gets worse as angular velocity increases. Start with a
+                      // coefficient of 0.1.
+        swerveDrive.setModuleEncoderAutoSynchronize(false,
+                1); // Enable if you want to resynchronize your absolute encoders and motor encoders
+                    // periodically when they are not moving.
+        // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used
+        // over the internal encoder and push the offsets onto it. Throws warning if not
+        // possible
+        if (visionDriveTest) {
+            setupPhotonVision();
+            // Stop the odometry thread if we are using vision that way we can synchronize
+            // updates better.
+            swerveDrive.stopOdometryThread();
+        }
+        setupPathPlanner();
+    }
+
+    /**
+     * Construct the swerve drive.
+     *
+     * @param driveCfg      SwerveDriveConfiguration for the swerve.
+     * @param controllerCfg Swerve Controller.
+     */
+    public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) {
+        swerveDrive = new SwerveDrive(driveCfg,
+                controllerCfg,
+                Constants.MAX_SPEED,
+                new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)),
+                        Rotation2d.fromDegrees(0)));
+    }
+
+    /**
+     * Setup the photon vision class.
+     */
+    public void setupPhotonVision() {
+        vision = new Vision(swerveDrive::getPose, swerveDrive.field);
+    }
+
+    @Override
+    public void periodic() {
+
+        // When vision is enabled we must manually update odometry in SwerveDrive
+        if (visionDriveTest) {
+            swerveDrive.updateOdometry();
+            vision.updatePoseEstimation(swerveDrive);
+        }
+    }
+
+    @Override
+    public void simulationPeriodic() {
+    }
+
+    /**
+     * Setup AutoBuilder for PathPlanner.
+     */
+    public void setupPathPlanner() {
+        // Load the RobotConfig from the GUI settings. You should probably
+        // store this in your Constants file
+        RobotConfig config;
+        try {
+            config = RobotConfig.fromGUISettings();
+
+            final boolean enableFeedforward = true;
+            // Configure AutoBuilder last
+            AutoBuilder.configure(
+                    this::getPose,
+                    // Robot pose supplier
+                    this::resetOdometry,
+                    // Method to reset odometry (will be called if your auto has a starting pose)
+                    this::getRobotVelocity,
+                    // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                    (speedsRobotRelative, moduleFeedForwards) -> {
+                        if (enableFeedforward) {
+                            swerveDrive.drive(
+                                    speedsRobotRelative,
+                                    swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative),
+                                    moduleFeedForwards.linearForces());
+                        } else {
+                            swerveDrive.setChassisSpeeds(speedsRobotRelative);
+                        }
+                    },
+                    // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also
+                    // optionally outputs individual module feedforwards
+                    new PPHolonomicDriveController(
+                            // PPHolonomicController is the built in path following controller for holonomic
+                            // drive trains
+                            new PIDConstants(5.0, 0.0, 0.0),
+                            // Translation PID constants
+                            new PIDConstants(3.8, 0.0, 0.0)
+                    // Rotation PID constants
+                    ),
+                    config,
+                    // The robot configuration
+                    () -> {
+                        // Boolean supplier that controls when the path will be mirrored for the red
+                        // alliance
+                        // This will flip the path being followed to the red side of the field.
+                        // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+                        var alliance = DriverStation.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == DriverStation.Alliance.Red;
+                        }
+                        return false;
+                    },
+                    this
+            // Reference to this subsystem to set requirements
+            );
+
+        } catch (Exception e) {
+            // Handle exception as needed
+            e.printStackTrace();
+        }
+
+        // Preload PathPlanner Path finding
+        // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE
+        PathfindingCommand.warmupCommand().schedule();
+    }
+
+    /**
+     * Aim the robot at the target returned by PhotonVision.
+     *
+     * @return A {@link Command} which will run the alignment.
+     */
+    public Command aimAtTarget(Cameras camera) {
+
+        return run(() -> {
+            Optional<PhotonPipelineResult> resultO = camera.getBestResult();
+            if (resultO.isPresent()) {
+                var result = resultO.get();
+                if (result.hasTargets()) {
+                    drive(getTargetSpeeds(0,
+                            0,
+                            Rotation2d.fromDegrees(result.getBestTarget()
+                                    .getYaw()))); // Not sure if this will work, more math may be required.
+                }
+            }
+        });
+    }
+
+    /**
+     * Get the path follower with events.
+     *
+     * @param pathName PathPlanner path name.
+     * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command.
+     */
+    public Command getAutonomousCommand(String pathName) {
+        // Create a path following command using AutoBuilder. This will also trigger
+        // event markers.
+        return new PathPlannerAuto(pathName);
+    }
+
+    /**
+     * Use PathPlanner Path finding to go to a point on the field.
+     *
+     * @param pose Target {@link Pose2d} to go to.
+     * @return PathFinding command
+     */
+    public Command driveToPose(Pose2d pose) {
+        // Create the constraints to use while pathfinding
+        PathConstraints constraints = new PathConstraints(
+                swerveDrive.getMaximumChassisVelocity(), 4.0,
+                swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720));
+
+        // Since AutoBuilder is configured, we can use it to build pathfinding commands
+        return AutoBuilder.pathfindToPose(
+                pose,
+                constraints,
+                edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
+        );
+    }
+
+    /**
+     * Drive with {@link SwerveSetpointGenerator} from 254, implemented by
+     * PathPlanner.
+     *
+     * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to
+     *                                  achieve.
+     * @return {@link Command} to run.
+     * @throws IOException    If the PathPlanner GUI settings is invalid
+     * @throws ParseException If PathPlanner GUI settings is nonexistent.
+     */
+    private Command driveWithSetpointGenerator(Supplier<ChassisSpeeds> robotRelativeChassisSpeed)
+            throws IOException, ParseException {
+        SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(),
+                swerveDrive.getMaximumChassisAngularVelocity());
+        AtomicReference<SwerveSetpoint> prevSetpoint = new AtomicReference<>(
+                new SwerveSetpoint(swerveDrive.getRobotVelocity(),
+                        swerveDrive.getStates(),
+                        DriveFeedforwards.zeros(swerveDrive.getModules().length)));
+        AtomicReference<Double> previousTime = new AtomicReference<>();
+
+        return startRun(() -> previousTime.set(Timer.getFPGATimestamp()),
+                () -> {
+                    double newTime = Timer.getFPGATimestamp();
+                    SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint(prevSetpoint.get(),
+                            robotRelativeChassisSpeed.get(),
+                            newTime - previousTime.get());
+                    swerveDrive.drive(newSetpoint.robotRelativeSpeeds(),
+                            newSetpoint.moduleStates(),
+                            newSetpoint.feedforwards().linearForces());
+                    prevSetpoint.set(newSetpoint);
+                    previousTime.set(newTime);
+
+                });
+    }
+
+    /**
+     * Drive with 254's Setpoint generator; port written by PathPlanner.
+     *
+     * @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds}
+     * @return Command to drive the robot using the setpoint generator.
+     */
+    public Command driveWithSetpointGeneratorFieldRelative(Supplier<ChassisSpeeds> fieldRelativeSpeeds) {
+        try {
+            return driveWithSetpointGenerator(() -> {
+                return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading());
+
+            });
+        } catch (Exception e) {
+            DriverStation.reportError(e.toString(), true);
+        }
+        return Commands.none();
+
+    }
+
+    /**
+     * Command to characterize the robot drive motors using SysId
+     *
+     * @return SysId Drive Command
+     */
+    public Command sysIdDriveMotorCommand() {
+        return SwerveDriveTest.generateSysIdCommand(
+                SwerveDriveTest.setDriveSysIdRoutine(
+                        new Config(),
+                        this, swerveDrive, 12, true),
+                3.0, 5.0, 3.0);
+    }
+
+    /**
+     * Command to characterize the robot angle motors using SysId
+     *
+     * @return SysId Angle Command
+     */
+    public Command sysIdAngleMotorCommand() {
+        return SwerveDriveTest.generateSysIdCommand(
+                SwerveDriveTest.setAngleSysIdRoutine(
+                        new Config(),
+                        this, swerveDrive),
+                3.0, 5.0, 3.0);
+    }
+
+    /**
+     * Returns a Command that centers the modules of the SwerveDrive subsystem.
+     *
+     * @return a Command that centers the modules of the SwerveDrive subsystem
+     */
+    public Command centerModulesCommand() {
+        return run(() -> Arrays.asList(swerveDrive.getModules())
+                .forEach(it -> it.setAngle(0.0)));
+    }
+
+    /**
+     * Returns a Command that tells the robot to drive forward until the command
+     * ends.
+     *
+     * @return a Command that tells the robot to drive forward until the command
+     *         ends
+     */
+    public Command driveForward() {
+        return run(() -> {
+            swerveDrive.drive(new Translation2d(1, 0), 0, false, false);
+        }).finallyDo(() -> swerveDrive.drive(new Translation2d(0, 0), 0, false, false));
+    }
+
+    /**
+     * Replaces the swerve module feedforward with a new SimpleMotorFeedforward
+     * object.
+     *
+     * @param kS the static gain of the feedforward
+     * @param kV the velocity gain of the feedforward
+     * @param kA the acceleration gain of the feedforward
+     */
+    public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) {
+        swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA));
+    }
+
+    /**
+     * Command to drive the robot using translative values and heading as angular
+     * velocity.
+     *
+     * @param translationX     Translation in the X direction. Cubed for smoother
+     *                         controls.
+     * @param translationY     Translation in the Y direction. Cubed for smoother
+     *                         controls.
+     * @param angularRotationX Angular velocity of the robot to set. Cubed for
+     *                         smoother controls.
+     * @return Drive command.
+     */
+    public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY,
+            DoubleSupplier angularRotationX) {
+        return run(() -> {
+            // Make the robot move
+            swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d(
+                    translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(),
+                    translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), 0.8),
+                    Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(),
+                    true,
+                    false);
+        });
+    }
+
+    /**
+     * Command to drive the robot using translative values and heading as a
+     * setpoint.
+     *
+     * @param translationX Translation in the X direction. Cubed for smoother
+     *                     controls.
+     * @param translationY Translation in the Y direction. Cubed for smoother
+     *                     controls.
+     * @param headingX     Heading X to calculate angle of the joystick.
+     * @param headingY     Heading Y to calculate angle of the joystick.
+     * @return Drive command.
+     */
+    public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX,
+            DoubleSupplier headingY) {
+        // swerveDrive.setHeadingCorrection(true); // Normally you would want heading
+        // correction for this kind of control.
+        return run(() -> {
+
+            Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(),
+                    translationY.getAsDouble()), 0.8);
+
+            // Make the robot move
+            driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(),
+                    headingX.getAsDouble(),
+                    headingY.getAsDouble(),
+                    swerveDrive.getOdometryHeading().getRadians(),
+                    swerveDrive.getMaximumChassisVelocity()));
+        });
+    }
+
+    /**
+     * The primary method for controlling the drivebase. Takes a
+     * {@link Translation2d} and a rotation rate, and
+     * calculates and commands module states accordingly. Can use either open-loop
+     * or closed-loop velocity control for
+     * the wheel velocities. Also has field- and robot-relative modes, which affect
+     * how the translation vector is used.
+     *
+     * @param translation   {@link Translation2d} that is the commanded linear
+     *                      velocity of the robot, in meters per
+     *                      second. In robot-relative mode, positive x is torwards
+     *                      the bow (front) and positive y is
+     *                      torwards port (left). In field-relative mode, positive x
+     *                      is away from the alliance wall
+     *                      (field North) and positive y is torwards the left wall
+     *                      when looking through the driver station
+     *                      glass (field West).
+     * @param rotation      Robot angular rate, in radians per second. CCW positive.
+     *                      Unaffected by field/robot
+     *                      relativity.
+     * @param fieldRelative Drive mode. True for field-relative, false for
+     *                      robot-relative.
+     */
+    public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
+        swerveDrive.drive(translation,
+                rotation,
+                fieldRelative,
+                false); // Open loop is disabled since it shouldn't be used most of the time.
+    }
+
+    /**
+     * Drive the robot given a chassis field oriented velocity.
+     *
+     * @param velocity Velocity according to the field.
+     */
+    public void driveFieldOriented(ChassisSpeeds velocity) {
+        swerveDrive.driveFieldOriented(velocity);
+    }
+
+    /**
+     * Drive the robot given a chassis field oriented velocity.
+     *
+     * @param velocity Velocity according to the field.
+     */
+    public Command driveFieldOriented(Supplier<ChassisSpeeds> velocity) {
+        return run(() -> {
+            swerveDrive.driveFieldOriented(velocity.get());
+        });
+    }
+
+    /**
+     * Drive according to the chassis robot oriented velocity.
+     *
+     * @param velocity Robot oriented {@link ChassisSpeeds}
+     */
+    public void drive(ChassisSpeeds velocity) {
+        swerveDrive.drive(velocity);
+    }
+
+    /**
+     * Get the swerve drive kinematics object.
+     *
+     * @return {@link SwerveDriveKinematics} of the swerve drive.
+     */
+    public SwerveDriveKinematics getKinematics() {
+        return swerveDrive.kinematics;
+    }
+
+    /**
+     * Resets odometry to the given pose. Gyro angle and module positions do not
+     * need to be reset when calling this
+     * method. However, if either gyro angle or module position is reset, this must
+     * be called in order for odometry to
+     * keep working.
+     *
+     * @param initialHolonomicPose The pose to set the odometry to
+     */
+    public void resetOdometry(Pose2d initialHolonomicPose) {
+        swerveDrive.resetOdometry(initialHolonomicPose);
+    }
+
+    /**
+     * Gets the current pose (position and rotation) of the robot, as reported by
+     * odometry.
+     *
+     * @return The robot's pose
+     */
+    public Pose2d getPose() {
+        return swerveDrive.getPose();
+    }
+
+    /**
+     * Set chassis speeds with closed-loop velocity control.
+     *
+     * @param chassisSpeeds Chassis Speeds to set.
+     */
+    public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
+        swerveDrive.setChassisSpeeds(chassisSpeeds);
+    }
+
+    /**
+     * Post the trajectory to the field.
+     *
+     * @param trajectory The trajectory to post.
+     */
+    public void postTrajectory(Trajectory trajectory) {
+        swerveDrive.postTrajectory(trajectory);
+    }
+
+    /**
+     * Resets the gyro angle to zero and resets odometry to the same position, but
+     * facing toward 0.
+     */
+    public void zeroGyro() {
+        swerveDrive.zeroGyro();
+    }
+
+    /**
+     * Checks if the alliance is red, defaults to false if alliance isn't available.
+     *
+     * @return true if the red alliance, false if blue. Defaults to false if none is
+     *         available.
+     */
+    private boolean isRedAlliance() {
+        var alliance = DriverStation.getAlliance();
+        return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
+    }
+
+    /**
+     * This will zero (calibrate) the robot to assume the current position is facing
+     * forward
+     * <p>
+     * If red alliance rotate the robot 180 after the drviebase zero command
+     */
+    public void zeroGyroWithAlliance() {
+        if (isRedAlliance()) {
+            zeroGyro();
+            // Set the pose 180 degrees
+            resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180)));
+        } else {
+            zeroGyro();
+        }
+    }
+
+    /**
+     * Sets the drive motors to brake/coast mode.
+     *
+     * @param brake True to set motors to brake mode, false for coast.
+     */
+    public void setMotorBrake(boolean brake) {
+        swerveDrive.setMotorIdleMode(brake);
+    }
+
+    /**
+     * Gets the current yaw angle of the robot, as reported by the swerve pose
+     * estimator in the underlying drivebase.
+     * Note, this is not the raw gyro reading, this may be corrected from calls to
+     * resetOdometry().
+     *
+     * @return The yaw angle
+     */
+    public Rotation2d getHeading() {
+        return getPose().getRotation();
+    }
+
+    /**
+     * Get the chassis speeds based on controller input of 2 joysticks. One for
+     * speeds in which direction. The other for
+     * the angle of the robot.
+     *
+     * @param xInput   X joystick input for the robot to move in the X direction.
+     * @param yInput   Y joystick input for the robot to move in the Y direction.
+     * @param headingX X joystick which controls the angle of the robot.
+     * @param headingY Y joystick which controls the angle of the robot.
+     * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
+     */
+    public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) {
+        Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
+        return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
+                scaledInputs.getY(),
+                headingX,
+                headingY,
+                getHeading().getRadians(),
+                Constants.MAX_SPEED);
+    }
+
+    /**
+     * Get the chassis speeds based on controller input of 1 joystick and one angle.
+     * Control the robot at an offset of
+     * 90deg.
+     *
+     * @param xInput X joystick input for the robot to move in the X direction.
+     * @param yInput Y joystick input for the robot to move in the Y direction.
+     * @param angle  The angle in as a {@link Rotation2d}.
+     * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
+     */
+    public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) {
+        Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
+
+        return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
+                scaledInputs.getY(),
+                angle.getRadians(),
+                getHeading().getRadians(),
+                Constants.MAX_SPEED);
+    }
+
+    /**
+     * Gets the current field-relative velocity (x, y and omega) of the robot
+     *
+     * @return A ChassisSpeeds object of the current field-relative velocity
+     */
+    public ChassisSpeeds getFieldVelocity() {
+        return swerveDrive.getFieldVelocity();
+    }
+
+    /**
+     * Gets the current velocity (x, y and omega) of the robot
+     *
+     * @return A {@link ChassisSpeeds} object of the current velocity
+     */
+    public ChassisSpeeds getRobotVelocity() {
+        return swerveDrive.getRobotVelocity();
+    }
+
+    /**
+     * Get the {@link SwerveController} in the swerve drive.
+     *
+     * @return {@link SwerveController} from the {@link SwerveDrive}.
+     */
+    public SwerveController getSwerveController() {
+        return swerveDrive.swerveController;
+    }
+
+    /**
+     * Get the {@link SwerveDriveConfiguration} object.
+     *
+     * @return The {@link SwerveDriveConfiguration} fpr the current drive.
+     */
+    public SwerveDriveConfiguration getSwerveDriveConfiguration() {
+        return swerveDrive.swerveDriveConfiguration;
+    }
+
+    /**
+     * Lock the swerve drive to prevent it from moving.
+     */
+    public void lock() {
+        swerveDrive.lockPose();
+    }
+
+    /**
+     * Gets the current pitch angle of the robot, as reported by the imu.
+     *
+     * @return The heading as a {@link Rotation2d} angle
+     */
+    public Rotation2d getPitch() {
+        return swerveDrive.getPitch();
+    }
+
+    /**
+     * Add a fake vision reading for testing purposes.
+     */
+    public void addFakeVisionReading() {
+        swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp());
+    }
+
+    /**
+     * Gets the swerve drive object.
+     *
+     * @return {@link SwerveDrive}
+     */
+    public SwerveDrive getSwerveDrive() {
+        return swerveDrive;
+    }
+}

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

@@ -0,0 +1,629 @@
+package frc.robot.subsystems.swervedrive;
+
+import static edu.wpi.first.units.Units.Microseconds;
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+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.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import frc.robot.Robot;
+import java.awt.Desktop;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import java.util.function.Supplier;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonUtils;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import swervelib.SwerveDrive;
+import swervelib.telemetry.SwerveDriveTelemetry;
+
+
+/**
+ * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from
+ * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads
+ */
+public class Vision
+{
+
+  /**
+   * April Tag Field Layout of the year.
+   */
+  public static final AprilTagFieldLayout fieldLayout                     = AprilTagFieldLayout.loadField(
+      AprilTagFields.k2026RebuiltAndymark);
+  /**
+   * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
+   */
+  private final       double              maximumAmbiguity                = 0.25;
+  /**
+   * Photon Vision Simulation
+   */
+  public              VisionSystemSim     visionSim;
+  /**
+   * Count of times that the odom thinks we're more than 10meters away from the april tag.
+   */
+  private             double              longDistangePoseEstimationCount = 0;
+  /**
+   * Current pose from the pose estimator using wheel odometry.
+   */
+  private             Supplier<Pose2d>    currentPose;
+  /**
+   * Field from {@link swervelib.SwerveDrive#field}
+   */
+  private             Field2d             field2d;
+
+
+  /**
+   * Constructor for the Vision class.
+   *
+   * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()}
+   * @param field       Current field, should be {@link SwerveDrive#field}
+   */
+  public Vision(Supplier<Pose2d> currentPose, Field2d field)
+  {
+    this.currentPose = currentPose;
+    this.field2d = field;
+
+    if (Robot.isSimulation())
+    {
+      visionSim = new VisionSystemSim("Vision");
+      visionSim.addAprilTags(fieldLayout);
+
+      for (Cameras c : Cameras.values())
+      {
+        c.addToVisionSim(visionSim);
+      }
+
+      openSimCameraViews();
+    }
+  }
+
+  /**
+   * Calculates a target pose relative to an AprilTag on the field.
+   *
+   * @param aprilTag    The ID of the AprilTag.
+   * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position
+   *                    itself correctly.
+   * @return The target pose of the AprilTag.
+   */
+  public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset)
+  {
+    Optional<Pose3d> aprilTagPose3d = fieldLayout.getTagPose(aprilTag);
+    if (aprilTagPose3d.isPresent())
+    {
+      return aprilTagPose3d.get().toPose2d().transformBy(robotOffset);
+    } else
+    {
+      throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString());
+    }
+
+  }
+
+  /**
+   * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses.
+   *
+   * @param swerveDrive {@link SwerveDrive} instance.
+   */
+  public void updatePoseEstimation(SwerveDrive swerveDrive)
+  {
+    if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent())
+    {
+      /*
+       * In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting.
+       * As a result, the odometry may not always be 100% accurate.
+       * However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect.
+       * (This is why teams implement vision system to correct odometry.)
+       * Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation.
+       */
+      visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
+    }
+    for (Cameras camera : Cameras.values())
+    {
+      Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
+      if (poseEst.isPresent())
+      {
+        var pose = poseEst.get();
+        swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(),
+                                         pose.timestampSeconds,
+                                         camera.curStdDevs);
+      }
+    }
+
+  }
+
+  /**
+   * Generates the estimated robot pose. Returns empty if:
+   * <ul>
+   *  <li> No Pose Estimates could be generated</li>
+   * <li> The generated pose estimate was considered not accurate</li>
+   * </ul>
+   *
+   * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate
+   */
+  public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera)
+  {
+    Optional<EstimatedRobotPose> poseEst = camera.getEstimatedGlobalPose();
+    if (Robot.isSimulation())
+    {
+      Field2d debugField = visionSim.getDebugField();
+      // Uncomment to enable outputting of vision targets in sim.
+      poseEst.ifPresentOrElse(
+          est ->
+              debugField
+                  .getObject("VisionEstimation")
+                  .setPose(est.estimatedPose.toPose2d()),
+          () -> {
+            debugField.getObject("VisionEstimation").setPoses();
+          });
+    }
+    return poseEst;
+  }
+
+
+  /**
+   * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than
+   * 10m for a short amount of time.
+   *
+   * @param pose Estimated robot pose.
+   * @return Could be empty if there isn't a good reading.
+   */
+  @Deprecated(since = "2024", forRemoval = true)
+  private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose)
+  {
+    if (pose.isPresent())
+    {
+      double bestTargetAmbiguity = 1; // 1 is max ambiguity
+      for (PhotonTrackedTarget target : pose.get().targetsUsed)
+      {
+        double ambiguity = target.getPoseAmbiguity();
+        if (ambiguity != -1 && ambiguity < bestTargetAmbiguity)
+        {
+          bestTargetAmbiguity = ambiguity;
+        }
+      }
+      //ambiguity to high dont use estimate
+      if (bestTargetAmbiguity > maximumAmbiguity)
+      {
+        return Optional.empty();
+      }
+
+      //est pose is very far from recorded robot pose
+      if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1)
+      {
+        longDistangePoseEstimationCount++;
+
+        //if it calculates that were 10 meter away for more than 10 times in a row its probably right
+        if (longDistangePoseEstimationCount < 10)
+        {
+          return Optional.empty();
+        }
+      } else
+      {
+        longDistangePoseEstimationCount = 0;
+      }
+      return pose;
+    }
+    return Optional.empty();
+  }
+
+
+  /**
+   * Get distance of the robot from the AprilTag pose.
+   *
+   * @param id AprilTag ID
+   * @return Distance
+   */
+  public double getDistanceFromAprilTag(int id)
+  {
+    Optional<Pose3d> tag = fieldLayout.getTagPose(id);
+    return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0);
+  }
+
+  /**
+   * Get tracked target from a camera of AprilTagID
+   *
+   * @param id     AprilTag ID
+   * @param camera Camera to check.
+   * @return Tracked target.
+   */
+  public PhotonTrackedTarget getTargetFromId(int id, Cameras camera)
+  {
+    PhotonTrackedTarget target = null;
+    for (PhotonPipelineResult result : camera.resultsList)
+    {
+      if (result.hasTargets())
+      {
+        for (PhotonTrackedTarget i : result.getTargets())
+        {
+          if (i.getFiducialId() == id)
+          {
+            return i;
+          }
+        }
+      }
+    }
+    return target;
+
+  }
+
+  /**
+   * Vision simulation.
+   *
+   * @return Vision Simulation
+   */
+  public VisionSystemSim getVisionSim()
+  {
+    return visionSim;
+  }
+
+  /**
+   * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost.
+   */
+  private void openSimCameraViews()
+  {
+    if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE))
+    {
+//      try
+//      {
+//        Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
+//        Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
+//        Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
+//      } catch (IOException | URISyntaxException e)
+//      {
+//        e.printStackTrace();
+//      }
+    }
+  }
+
+  /**
+   * Update the {@link Field2d} to include tracked targets/
+   */
+  public void updateVisionField()
+  {
+
+    List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>();
+    for (Cameras c : Cameras.values())
+    {
+      if (!c.resultsList.isEmpty())
+      {
+        PhotonPipelineResult latest = c.resultsList.get(0);
+        if (latest.hasTargets())
+        {
+          targets.addAll(latest.targets);
+        }
+      }
+    }
+
+    List<Pose2d> poses = new ArrayList<>();
+    for (PhotonTrackedTarget target : targets)
+    {
+      if (fieldLayout.getTagPose(target.getFiducialId()).isPresent())
+      {
+        Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d();
+        poses.add(targetPose);
+      }
+    }
+
+    field2d.getObject("tracked targets").setPoses(poses);
+  }
+
+  /**
+   * Camera Enum to select each camera
+   */
+  enum Cameras
+  {
+    /**
+     * Left Camera
+     */
+    LEFT_CAM("left",
+             new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
+             new Translation3d(Units.inchesToMeters(12.056),
+                               Units.inchesToMeters(10.981),
+                               Units.inchesToMeters(8.44)),
+             VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
+    /**
+     * Right Camera
+     */
+    RIGHT_CAM("right",
+              new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
+              new Translation3d(Units.inchesToMeters(12.056),
+                                Units.inchesToMeters(-10.981),
+                                Units.inchesToMeters(8.44)),
+              VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
+    /**
+     * Center Camera
+     */
+    CENTER_CAM("center",
+               new Rotation3d(0, Units.degreesToRadians(18), 0),
+               new Translation3d(Units.inchesToMeters(-4.628),
+                                 Units.inchesToMeters(-10.687),
+                                 Units.inchesToMeters(16.129)),
+               VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
+
+    /**
+     * Latency alert to use when high latency is detected.
+     */
+    public final  Alert                        latencyAlert;
+    /**
+     * Camera instance for comms.
+     */
+    public final  PhotonCamera                 camera;
+    /**
+     * Pose estimator for camera.
+     */
+    public final  PhotonPoseEstimator          poseEstimator;
+    /**
+     * Standard Deviation for single tag readings for pose estimation.
+     */
+    private final Matrix<N3, N1>               singleTagStdDevs;
+    /**
+     * Standard deviation for multi-tag readings for pose estimation.
+     */
+    private final Matrix<N3, N1>               multiTagStdDevs;
+    /**
+     * Transform of the camera rotation and translation relative to the center of the robot
+     */
+    private final Transform3d                  robotToCamTransform;
+    /**
+     * Current standard deviations used.
+     */
+    public        Matrix<N3, N1>               curStdDevs;
+    /**
+     * Estimated robot pose.
+     */
+    public        Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
+
+    /**
+     * Simulated camera instance which only exists during simulations.
+     */
+    public        PhotonCameraSim              cameraSim;
+    /**
+     * Results list to be updated periodically and cached to avoid unnecessary queries.
+     */
+    public        List<PhotonPipelineResult>   resultsList       = new ArrayList<>();
+    /**
+     * Last read from the camera timestamp to prevent lag due to slow data fetches.
+     */
+    private       double                       lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
+
+    /**
+     * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine
+     * estimation noise on an actual robot.
+     *
+     * @param name                  Name of the PhotonVision camera found in the PV UI.
+     * @param robotToCamRotation    {@link Rotation3d} of the camera.
+     * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot.
+     * @param singleTagStdDevs      Single AprilTag standard deviations of estimated poses from the camera.
+     * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera.
+     */
+    Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation,
+            Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix)
+    {
+      latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning);
+
+      camera = new PhotonCamera(name);
+
+      // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html
+      robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation);
+
+      poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout,
+                                              PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+                                              robotToCamTransform);
+      poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+
+      this.singleTagStdDevs = singleTagStdDevs;
+      this.multiTagStdDevs = multiTagStdDevsMatrix;
+
+      if (Robot.isSimulation())
+      {
+        SimCameraProperties cameraProp = new SimCameraProperties();
+        // A 640 x 480 camera with a 100 degree diagonal FOV.
+        cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100));
+        // Approximate detection noise with average and standard deviation error in pixels.
+        cameraProp.setCalibError(0.25, 0.08);
+        // Set the camera image capture framerate (Note: this is limited by robot loop rate).
+        cameraProp.setFPS(30);
+        // The average and standard deviation in milliseconds of image data latency.
+        cameraProp.setAvgLatencyMs(35);
+        cameraProp.setLatencyStdDevMs(5);
+
+        cameraSim = new PhotonCameraSim(camera, cameraProp);
+        cameraSim.enableDrawWireframe(true);
+      }
+    }
+
+    /**
+     * Add camera to {@link VisionSystemSim} for simulated photon vision.
+     *
+     * @param systemSim {@link VisionSystemSim} to use.
+     */
+    public void addToVisionSim(VisionSystemSim systemSim)
+    {
+      if (Robot.isSimulation())
+      {
+        systemSim.addCamera(cameraSim, robotToCamTransform);
+      }
+    }
+
+    /**
+     * Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most
+     * recent result!
+     *
+     * @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result!
+     */
+    public Optional<PhotonPipelineResult> getBestResult()
+    {
+      if (resultsList.isEmpty())
+      {
+        return Optional.empty();
+      }
+
+      PhotonPipelineResult bestResult       = resultsList.get(0);
+      double               amiguity         = bestResult.getBestTarget().getPoseAmbiguity();
+      double               currentAmbiguity = 0;
+      for (PhotonPipelineResult result : resultsList)
+      {
+        currentAmbiguity = result.getBestTarget().getPoseAmbiguity();
+        if (currentAmbiguity < amiguity && currentAmbiguity > 0)
+        {
+          bestResult = result;
+          amiguity = currentAmbiguity;
+        }
+      }
+      return Optional.of(bestResult);
+    }
+
+    /**
+     * Get the latest result from the current cache.
+     *
+     * @return Empty optional if nothing is found. Latest result if something is there.
+     */
+    public Optional<PhotonPipelineResult> getLatestResult()
+    {
+      return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0));
+    }
+
+    /**
+     * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the
+     * cache of results.
+     *
+     * @return Estimated pose.
+     */
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose()
+    {
+      updateUnreadResults();
+      return estimatedRobotPose;
+    }
+
+    /**
+     * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp.
+     */
+    private void updateUnreadResults()
+    {
+      double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds();
+      
+      for (PhotonPipelineResult result : resultsList)
+      {
+        mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
+      }
+
+        resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
+        resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
+          return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
+        });
+        if (!resultsList.isEmpty())
+        {
+          updateEstimatedGlobalPose();
+        }
+
+    }
+
+    /**
+     * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once
+     * per loop.
+     *
+     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
+     * {@link Cameras#updateEstimationStdDevs}
+     *
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for
+     * estimation.
+     */
+    private void updateEstimatedGlobalPose()
+    {
+      Optional<EstimatedRobotPose> visionEst = Optional.empty();
+      for (var change : resultsList)
+      {
+        visionEst = poseEstimator.update(change);
+        updateEstimationStdDevs(visionEst, change.getTargets());
+      }
+      estimatedRobotPose = visionEst;
+    }
+
+    /**
+     * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based
+     * on number of tags, estimation strategy, and distance from the tags.
+     *
+     * @param estimatedPose The estimated pose to guess standard deviations for.
+     * @param targets       All targets in this camera frame
+     */
+    private void updateEstimationStdDevs(
+        Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets)
+    {
+      if (estimatedPose.isEmpty())
+      {
+        // No pose input. Default to single-tag std devs
+        curStdDevs = singleTagStdDevs;
+
+      } else
+      {
+        // Pose present. Start running Heuristic
+        var    estStdDevs = singleTagStdDevs;
+        int    numTags    = 0;
+        double avgDist    = 0;
+
+        // Precalculation - see how many tags we found, and calculate an average-distance metric
+        for (var tgt : targets)
+        {
+          var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+          if (tagPose.isEmpty())
+          {
+            continue;
+          }
+          numTags++;
+          avgDist +=
+              tagPose
+                  .get()
+                  .toPose2d()
+                  .getTranslation()
+                  .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+        }
+
+        if (numTags == 0)
+        {
+          // No tags visible. Default to single-tag std devs
+          curStdDevs = singleTagStdDevs;
+        } else
+        {
+          // One or more tags visible, run the full heuristic.
+          avgDist /= numTags;
+          // Decrease std devs if multiple targets are visible
+          if (numTags > 1)
+          {
+            estStdDevs = multiTagStdDevs;
+          }
+          // Increase std devs based on (average) distance
+          if (numTags == 1 && avgDist > 4)
+          {
+            estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+          } else
+          {
+            estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+          }
+          curStdDevs = estStdDevs;
+        }
+      }
+    }
+  }
+}