Delete unnecessary files. Clean up code. Change over to BangBangController + Feedforward for Velocity Control. Change over to PIDController + Feedforward for Position Control.

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

@@ -4,17 +4,13 @@
 
 package frc.robot;
 
-import org.photonvision.PhotonCamera;
+import edu.wpi.first.math.VecBuilder;
-import org.photonvision.PhotonPoseEstimator;
+import edu.wpi.first.math.Vector;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
 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.Translation3d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
@@ -41,7 +37,7 @@ public final class Constants {
     public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound
     public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
     public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag
-    public static final double MAX_SPEED = Units.feetToMeters(14.5);
+    public static final double MAX_SPEED_MPS = Units.feetToMeters(14.5);
     // Maximum speed of the robot in meters per second, used to limit acceleration.
 
     // public static final class AutonConstants
@@ -81,14 +77,25 @@ public final class Constants {
         public static final int LEFT_SHOOTER_MOTOR_ID = 41;
         public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
         public static final int INDEXER_MOTOR_ID = 43;
+        
+        public static final int SHOOTER_MOTOR_CURRENT_LIMIT = 80;
 
-        public static double SHOOTER_MOTOR_P = 0.0018;
+        public static final double SHOOTER_MOTOR_P = 0.0018;
-        public static double SHOOTER_MOTOR_I = 0;
+        public static final double SHOOTER_MOTOR_I = 0;
-        public static double SHOOTER_MOTOR_D = 0;
+        public static final double SHOOTER_MOTOR_D = 0;
 
-        public static double INDEXER_MOTOR_P = 0.0001;
+        public static final double INDEXER_MOTOR_P = 0.0001;
-        public static double INDEXER_MOTOR_I = 0;
+        public static final double INDEXER_MOTOR_I = 0;
-        public static double INDEXER_MOTOR_D = 0;
+        public static final double INDEXER_MOTOR_D = 0;
+
+        public static final double CENTER_MOTOR_S = 0.0;
+        public static final double CENTER_MOTOR_V = 0.0;
+
+        public static final double LEFT_MOTOR_S = 0.0;
+        public static final double LEFT_MOTOR_V = 0.0;
+
+        public static final double RIGHT_MOTOR_S = 0.0;
+        public static final double RIGHT_MOTOR_V = 0.0;
 
 
         /*private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer RPM", 2000)
@@ -115,20 +122,37 @@ public final class Constants {
         }*/
 
         public static final int INTAKE_WHEELS_MOTOR_ID = 50;
+        public static final int INTAKE_WHEELS_CURRENT_LIMIT = 60;
+        public static final double INTAKE_WHEELS_POSITION_CONVERSION_FACTOR = 2 * Math.PI; // Encoder Unit * Conversion Factor = Radians. 1 Rotation = 2PI Radians
+        public static final double INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR = INTAKE_WHEELS_POSITION_CONVERSION_FACTOR * 60.0; // Encoder Units per Minute * Conversion Factor = Radians per Second
+
         public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
+        public static final int INTAKE_ROTATOR_CURRENT_LIMIT = 40;
+        public static final double INTAKE_ROTATOR_INITIAL_ENCODER_VALUE = Units.degreesToRadians(-90);
+        public static final double INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR = (2 * Math.PI) / 12.0; // Encoder Unit * Conversion Factor = Radians. 1 Rotation = 2PI Radians. Gear ratio is 12:1, so 12 Rotations = 1 full intake Rotation.
+        public static final double INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR = INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR * 60.0; // Encoder Units per Minute * Conversion Factor = Radians per Second
 
-        public static class IntakeRotatorPID {
+        public static final double INTAKE_WHEELS_MOTOR_P = 0.0001; // Radians -> Radians Per Second
-            public static final double INTAKE_ROTATOR_P = 0.03;
+        public static final double INTAKE_WHEELS_MOTOR_I = 0.0; // Radians -> Radians Per Second
-            public static final double INTAKE_ROTATOR_I = 0;
+        public static final double INTAKE_WHEELS_MOTOR_D = 0.0; // Radians -> Radians Per Second
-            public static final double INTAKE_ROTATOR_D = 0;
+        public static final double INTAKE_WHEELS_MOTOR_S = 0.0; // Voltage to overcome static friction/inertia
-        }
+        public static final double INTAKE_WHEELS_MOTOR_V = 0.0; // Voltage per Rads/Second gain
-        public static final double INTAKE_MOTOR_P = 0.0001;
-        public static final double INTAKE_MOTOR_I = 0;
-        public static final double INTAKE_MOTOR_D = 0;
 
-        public static final double INTAKE_COLLECT_ENCODER_VALUE = 5;
+        public static final double INTAKE_ROTATOR_DOWN_P = 0.03; // Radians -> Radians Per Second
-        public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5;
+        public static final double INTAKE_ROTATOR_DOWN_I = 0.0; // Radians -> Radians Per Second
-        public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
+        public static final double INTAKE_ROTATOR_DOWN_D = 0.0; // Radians -> Radians Per Second
+
+        public static final double INTAKE_ROTATOR_UP_P = 0.06; // Radians -> Radians Per Second
+        public static final double INTAKE_ROTATOR_UP_I = 0.0; // Radians -> Radians Per Second
+        public static final double INTAKE_ROTATOR_UP_D = 0.0; // Radians -> Radians Per Second
+
+        public static final double INTAKE_ROTATOR_MOTOR_S = 0.0; // Voltage to overcome static friction/inertia. V
+        public static final double INTAKE_ROTATOR_MOTOR_G = 0.0; // Voltage to overcome Gravity. V
+        public static final double INTAKE_ROTATOR_MOTOR_V = 0.0; // Voltage per Radians/Second. V/(Rad/s)
+
+        public static final double INTAKE_COLLECT_POSITION_RADS = 0.0; 
+        public static final double INTAKE_MIDDLE_POSITION_RADS = -Units.degreesToRadians(45);
+        public static final double INTAKE_RETRACT_POSITION_RADS = -Units.degreesToRadians(90);
     }
 
 
@@ -138,45 +162,31 @@ public final class Constants {
 
 
     public static class TargetingConstants {
-        public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
+        public static final Pose2d RIGHT_CLIMB_POSE_METERS = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
-        public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
+        public static final Pose2d LEFT_CLIMB_POSE_METERS = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
 
-        public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Rear Left Camera");
+        public static final Vector<N3> SINGLE_TAG_STD_DEVS = VecBuilder.fill(4, 4, 8);
-        public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Rear Right Camera");
+        public static final Vector<N3> MULTI_TAG_STD_DEVS = VecBuilder.fill(0.5, 0.5, 1);
-        public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera");
-        public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera");
 
-        public static final Pose2d HUB_POSE = new Pose2d(4.625, 4.03, new Rotation2d());
+        public static final Pose2d HUB_POSE_METERS = new Pose2d(4.625, 4.03, new Rotation2d());
 
-        public static final Transform3d ORANGE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
+        public static final Translation3d FRONT_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
-                new Rotation3d(0, 0, 0));
+        public static final Translation3d FRONT_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
-        public static final Transform3d BLACK_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
+        public static final Translation3d REAR_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
-                new Rotation3d(0, 0, 0));
+        public static final Translation3d REAR_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
-        public static final Transform3d RED_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
-                new Rotation3d(0, 0, 0));
-        public static final Transform3d PURPLE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
-                new Rotation3d(0, 0, 0));
 
-        public static final PhotonPoseEstimator ORANGE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
+        public static final Rotation3d FRONT_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
-                AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
+        public static final Rotation3d FRONT_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
-                ORANGE_ROBOT_TO_CAM);
+        public static final Rotation3d REAR_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
-        public static final PhotonPoseEstimator BLACK_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
+        public static final Rotation3d REAR_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
-                AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
-                BLACK_ROBOT_TO_CAM);
-        public static final PhotonPoseEstimator RED_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
-                AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
-                RED_ROBOT_TO_CAM);
-        public static final PhotonPoseEstimator PURPLE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
-                AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
-                PURPLE_ROBOT_TO_CAM);
     }
 
     public static class ClimberConstants {
         public static final int CLIMB_MOTOR_ID = 60;
         public static final int RATCHET_PWM_PORT = 9;
 
-        public static final double RATCHET_UNLOCK_ANGLE = 0;
+        public static final double RATCHET_UNLOCK_ANGLE_DEGREES = 0;
-        public static final double RATCHET_LOCK_ANGLE = 180;
+        public static final double RATCHET_LOCK_ANGLE_DEGREES = 180;
-        public static final double CLIMBER_SPEED = .5;
+        public static final double CLIMBER_SPEED_DUTY_CYCLE = .5;
     }
 }

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

@@ -4,22 +4,11 @@
 
 package frc.robot;
 
-import org.photonvision.targeting.PhotonPipelineResult;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.subsystems.TargetingSubsystems;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -35,8 +24,6 @@ public class Robot extends TimedRobot {
     private RobotContainer m_robotContainer;
 
     private Timer disabledTimer;
-    private static NetworkTable table;
-    private static GenericEntry distanceFromLimelight;
 
 
     public Robot() {
@@ -95,16 +82,6 @@ public class Robot extends TimedRobot {
         //Constants.IntakeConstants.updateIntakeWheelsRPM();
         Constants.ShooterConstants.updateShooterRPM();
         //Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
-        
-        TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM,
-                Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
-        TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.BLACK_PHOTON_CAM,
-                Constants.TargetingConstants.BLACK_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
-        TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.RED_PHOTON_CAM,
-                Constants.TargetingConstants.RED_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
-        TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.PURPLE_PHOTON_CAM,
-                Constants.TargetingConstants.PURPLE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
-
     }
 
     /**

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

@@ -7,8 +7,6 @@ package frc.robot;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -18,7 +16,6 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -30,13 +27,7 @@ import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import java.io.File;
-import java.lang.annotation.Target;
-import java.util.concurrent.atomic.AtomicBoolean;
-import java.util.function.DoubleSupplier;
 
-import javax.lang.model.util.ElementScanner14;
-
-import frc.robot.subsystems.TargetingSubsystems;
 import swervelib.SwerveInputStream;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.ClimberSubsystem;

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

@@ -8,16 +8,16 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
-public class ClimberSubsystem extends SubsystemBase{
+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);
+        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE);
     }
 
     public void lowerRobot() {
-        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
+        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE * -1);
     }
 
     public void stopClimber() {
@@ -37,11 +37,11 @@ public class ClimberSubsystem extends SubsystemBase{
     }
 
     public static void toggleRatchet(boolean toggle) {
-    if (toggle == true) {
+      if (toggle == true) {
-      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
+        climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE_DEGREES);
-    } else
+      } else
-      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
+        climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE_DEGREES);
-  }
+    }
 
   public Command toggleRatchetCommand(boolean toggle) {
     return runOnce(() -> toggleRatchet(toggle));

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

@@ -1,66 +1,65 @@
 package frc.robot.subsystems;
 
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.BangBangController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 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 intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
+    private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID, MotorType.kBrushless);
-            MotorType.kBrushless);
 
-    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
+    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, MotorType.kBrushless);
-            MotorType.kBrushless);
-    private static SparkClosedLoopController intakeRotatorPIDController;
-    public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
 
-    private static SparkClosedLoopController intakeWheelsMotorPIDController;
     public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
+    private static BangBangController intakeWheelsMotorBBController = new BangBangController();
+    private static SimpleMotorFeedforward intakeWheelsMotorFeedforward;
+
+    public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
+    private static PIDController intakeRotatorMotorPIDController;
+    private static ArmFeedforward intakeRotatorMotorFeedforward;
 
     public IntakeSubsystem() {
-        intakeRotatorConfig.closedLoop
+        intakeWheelsMotorConfig
-        .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P)
+            .smartCurrentLimit(Constants.IntakeConstants.INTAKE_WHEELS_CURRENT_LIMIT)
-        .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
+            .encoder
-        .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
+            .positionConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_POSITION_CONVERSION_FACTOR)
+            .velocityConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR);
+        intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
 
-        .p(.06, ClosedLoopSlot.kSlot1)
+        intakeWheelsMotorFeedforward = new SimpleMotorFeedforward(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_S, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
-        .i(0, ClosedLoopSlot.kSlot1)
+        
-        .d(0, ClosedLoopSlot.kSlot1);
+        intakeRotatorConfig
-        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+            .smartCurrentLimit(Constants.IntakeConstants.INTAKE_ROTATOR_CURRENT_LIMIT)
-                com.revrobotics.PersistMode.kNoPersistParameters);
+            .encoder
-        intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
+            .positionConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR)
+            .velocityConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR);
+        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
+        intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_ROTATOR_INITIAL_ENCODER_VALUE);
 
-        intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P,
+        intakeRotatorMotorPIDController = new PIDController(Constants.IntakeConstants.INTAKE_ROTATOR_UP_P, Constants.IntakeConstants.INTAKE_ROTATOR_UP_I, Constants.IntakeConstants.INTAKE_ROTATOR_UP_D);
-                Constants.IntakeConstants.INTAKE_MOTOR_I,
+        intakeRotatorMotorFeedforward = new ArmFeedforward(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_S, Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_G, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
-                Constants.IntakeConstants.INTAKE_MOTOR_D);
-        intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-                com.revrobotics.PersistMode.kNoPersistParameters);
-        intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
     }
 
     public void startIntakeMotor() {
-        intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity);
+        intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM);
     }
 
     public void reverseIntakeMotor() {
-        intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity);
+        intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1);
     }
 
     public void stopIntakeMotor() {
-        intakeWheelsMotor.set(0);
+        intakeWheelsMotorBBController.setSetpoint(0.0);
     }
 
     public Command startIntakeMotorCommand() {
@@ -76,7 +75,7 @@ public class IntakeSubsystem extends SubsystemBase {
     }
 
     public void deployIntake() {
-        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0);
+        intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_POSITION_RADS);
     }
 
     public Command deployintakeCommand() {
@@ -84,7 +83,7 @@ public class IntakeSubsystem extends SubsystemBase {
     }
 
     public void retractIntake() {
-        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1);
+        intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_POSITION_RADS);
     }
 
     public Command retractIntakeCommand() {
@@ -92,8 +91,7 @@ public class IntakeSubsystem extends SubsystemBase {
     }
 
     public void assistFuelIntake() {
-        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
+        intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_POSITION_RADS);
-                ControlType.kPosition);
     }
 
     public Command assistFuelIntakeCommand() {
@@ -103,6 +101,13 @@ public class IntakeSubsystem extends SubsystemBase {
 
     @Override
     public void periodic() {
+        intakeWheelsMotor.setVoltage(
+            12.0 * intakeWheelsMotorBBController.calculate(intakeWheelsMotor.getEncoder().getVelocity()) 
+            + 0.9 * intakeWheelsMotorFeedforward.calculate(intakeWheelsMotorBBController.getSetpoint()));
+        intakeRotatorMotor.setVoltage(
+            12.0 * intakeRotatorMotorPIDController.calculate(intakeRotatorMotor.getEncoder().getPosition()) 
+            + intakeRotatorMotorFeedforward.calculate(intakeRotatorMotorPIDController.getSetpoint(), 0.0));
+
         SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
     }
 }

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

@@ -1,30 +1,17 @@
 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.SparkClosedLoopController;
-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.math.controller.BangBangController;
-import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 
 public class ShooterSubsystem extends SubsystemBase {
     private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
@@ -39,46 +26,41 @@ public class ShooterSubsystem extends SubsystemBase {
     private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
             MotorType.kBrushless);
 
-    private static SparkClosedLoopController centerShooterMotorPIDController;
     public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig();
+    private static BangBangController centerShooterMotorBBController = new BangBangController();
+    private static SimpleMotorFeedforward centerShooterMotorFeedforward;
 
-    private static SparkClosedLoopController leftShooterMotorPIDController;
     public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
+    private static BangBangController leftShooterMotorBBController = new BangBangController();
+    private static SimpleMotorFeedforward leftShooterMotorFeedforward;
 
-    private static SparkClosedLoopController rightShooterMotorPIDController;
     public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig();
+    private static BangBangController rightShooterMotorBBController = new BangBangController();
+    private static SimpleMotorFeedforward rightShooterMotorFeedforward;
 
-    private static SparkClosedLoopController indexerAndRampMotorPIDController;
     public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig();
+    private static PIDController indexerAndRampMotorPIDController;
 
     public ShooterSubsystem() {
-        centerShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
+        centerShooterMotorConfig.smartCurrentLimit(60);
-                Constants.ShooterConstants.SHOOTER_MOTOR_I,
-                Constants.ShooterConstants.SHOOTER_MOTOR_D);
         centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-                com.revrobotics.PersistMode.kNoPersistParameters);
+            com.revrobotics.PersistMode.kNoPersistParameters);
-        centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
+        centerShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.CENTER_MOTOR_S, Constants.ShooterConstants.CENTER_MOTOR_V);
     
-    leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
+        leftShooterMotorConfig.smartCurrentLimit(60);
-                Constants.ShooterConstants.SHOOTER_MOTOR_I,
-                Constants.ShooterConstants.SHOOTER_MOTOR_D);
         leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-                com.revrobotics.PersistMode.kNoPersistParameters);
+            com.revrobotics.PersistMode.kNoPersistParameters);
-        leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController();
+        leftShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.LEFT_MOTOR_S, Constants.ShooterConstants.LEFT_MOTOR_V);
     
-    rightShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
+        rightShooterMotorConfig.smartCurrentLimit(60);
-                Constants.ShooterConstants.SHOOTER_MOTOR_I,
-                Constants.ShooterConstants.SHOOTER_MOTOR_D);
         rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-                com.revrobotics.PersistMode.kNoPersistParameters);
+            com.revrobotics.PersistMode.kNoPersistParameters);
-        rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController();
+        rightShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.RIGHT_MOTOR_S, Constants.ShooterConstants.RIGHT_MOTOR_V);
 
-        indexerAndRampMotorConfig.closedLoop.pid(0.0001,
+        indexerAndRampMotorConfig.smartCurrentLimit(60);
-                0,
-                0);
         indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-                com.revrobotics.PersistMode.kNoPersistParameters);
+            com.revrobotics.PersistMode.kNoPersistParameters);
-        indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController();
+        indexerAndRampMotorPIDController = new PIDController(Constants.ShooterConstants.INDEXER_MOTOR_P, Constants.ShooterConstants.INDEXER_MOTOR_I, Constants.ShooterConstants.INDEXER_MOTOR_D);
     }
 
 
@@ -94,49 +76,49 @@ public class ShooterSubsystem extends SubsystemBase {
     private static double currentPotentiometerPosition; // might need second value for the right potentiometer
 
     public void setShooterMotorsRPM() {
-        centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
+        centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-        leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
+        leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-        rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
+        rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
     }
 
     //test individual motor code
     public void setLeftShooterMotorRPM() {
-        leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
+        leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
     }
     public Command testLeftShooterCommand() {
         return runOnce(() -> setLeftShooterMotorRPM());
     }
 
     public void setRightShooterMotorRPM() {
-        rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
+        rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
     }
     public Command testRightShooterCommand() {
         return runOnce(() -> setRightShooterMotorRPM());
     }
 
     public void setCenterShooterMotorRPM() {
-        centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
+        centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
     }
     public Command testCenterShooterCommand() {
         return runOnce(() -> setCenterShooterMotorRPM());
     }
 
     public void stopLeftShooterMotorRPM() {
-        leftShooterMotor.set(0);    
+        leftShooterMotorBBController.setSetpoint(0.0);
     }
     public Command stopLeftShooterCommand() {
         return runOnce(() -> stopLeftShooterMotorRPM());
     }
 
     public void stopCenterShooterMotorRPM() {
-        centerShooterMotor.set(0);    
+        centerShooterMotorBBController.setSetpoint(0.0);
     }
     public Command stopCenterShooterCommand() {
         return runOnce(() -> stopCenterShooterMotorRPM());
     }
 
     public void stopRightShooterMotorRPM() {
-        rightShooterMotor.set(0);    
+        rightShooterMotorBBController.setSetpoint(0.0);
     }
     public Command stopRightShooterCommand() {
         return runOnce(() -> stopRightShooterMotorRPM());
@@ -148,11 +130,11 @@ public class ShooterSubsystem extends SubsystemBase {
     }
 
     public void setIndexerAndRampMotorRPM() {
-        indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity);
+        indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM);
     }
 
     public void reverseIndexerAndRampMotorRPM() {
-        indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, ControlType.kVelocity);
+        indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1);
     }
 
     public Command reverseIndexerAndRampMotorRPMCommand() {
@@ -164,7 +146,7 @@ public class ShooterSubsystem extends SubsystemBase {
     }
 
     public Command stopIndexerAndRampMotorCommand() {
-        return runOnce(()-> indexerAndRampMotor.set(0));
+        return runOnce(()-> indexerAndRampMotorPIDController.setSetpoint(0.0));
     }
 
     /*public Command shootFuelCommand() {
@@ -181,12 +163,10 @@ public class ShooterSubsystem extends SubsystemBase {
      
 
     public void stopShooters() {
-
+        centerShooterMotorBBController.setSetpoint(0.0);
-        centerShooterMotor.set(0);
+        leftShooterMotorBBController.setSetpoint(0.0);
-        leftShooterMotor.set(0);
+        rightShooterMotorBBController.setSetpoint(0.0);
-        rightShooterMotor.set(0);
+        indexerAndRampMotorPIDController.setSetpoint(0.0);
-        indexerAndRampMotor.set(0);
-
     }
 
     public Command stopShooterCommand() {
@@ -217,6 +197,17 @@ public class ShooterSubsystem extends SubsystemBase {
 
     @Override
     public void periodic() {
+        centerShooterMotor.setVoltage(
+            12.0 * centerShooterMotorBBController.calculate(centerShooterMotor.getEncoder().getVelocity())
+            + 0.9 * centerShooterMotorFeedforward.calculate(centerShooterMotorBBController.getSetpoint()));
+        leftShooterMotor.setVoltage(
+            12.0 * leftShooterMotorBBController.calculate(leftShooterMotor.getEncoder().getVelocity())
+            + 0.9 * leftShooterMotorFeedforward.calculate(leftShooterMotorBBController.getSetpoint()));
+        rightShooterMotor.setVoltage(
+            12.0 * rightShooterMotorBBController.calculate(rightShooterMotor.getEncoder().getVelocity())
+            + 0.9 * rightShooterMotorFeedforward.calculate(rightShooterMotorBBController.getSetpoint()));
+        indexerAndRampMotor.setVoltage(
+            12.0 * indexerAndRampMotorPIDController.calculate(indexerAndRampMotor.getEncoder().getVelocity()));
 
         SmartDashboard.putNumber("Shooter Velocity", leftShooterMotor.getEncoder().getVelocity());
         /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());

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

@@ -1,207 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.List;
-import java.util.Optional;
-import java.util.function.DoubleSupplier;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonUtils;
-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 {
-
-    PIDController photonAimPIDController = new PIDController(3, 0, 0.001);
-
-    public TargetingSubsystems() {
-        photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI);
-    }
-
-    Pose2d currentRobotPose;
-
-    public List<Waypoint> pathWaypoints;
-
-    public Command pathPlanToPoseCommand(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
-        GoalEndState goalEndState = new GoalEndState(0, desiredPose.getRotation());
-        PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
-        currentRobotPose = swerveDrive.getPose();
-        pathWaypoints = PathPlannerPath.waypointsFromPoses(
-                currentRobotPose, desiredPose);
-
-        PathPlannerPath goToDesiredPose = new PathPlannerPath(pathWaypoints, pathConstraints, null,
-                goalEndState);
-        goToDesiredPose.preventFlipping = true;
-
-        return swerveDrive.getAutonomousCommand("goToDesiredPose");
-    }
-
-    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);
-
-            double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
-            double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
-            double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
-                    desiredPose.getRotation().getRadians());
-
-            swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, false);
-        }, swerveDrive);
-    }
-
-    public Command aimAtHub(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
-        return new RunCommand(() -> {
-            currentRobotPose = swerveDrive.getPose();
-
-            // Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
-
-            Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose,
-                    Constants.TargetingConstants.HUB_POSE);
-
-            double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
-                    angleDifference.getRadians());
-
-            angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0);
-
-            swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1, -driverXbox.getLeftY() * -1), angleSpeed,
-                    false);
-        }, swerveDrive);
-    }
-
-    Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
-        return new RunCommand(() -> {
-            double rot = 0.0;
-            var result = Constants.TargetingConstants.RED_PHOTON_CAM.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.getLeftX() * -1,
-                    driverXbox.getLeftY() * -1), rot, false);
-        }, swerveDrive);
-    }
-
-    public PhotonPoseEstimator getPhotonPoseEstimator(PhotonPoseEstimator poseEstimator) {
-        return poseEstimator;
-    }
-
-    /*
-     * // 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 static Optional<EstimatedRobotPose> visionEst = Optional.empty();
-
-    public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) {
-     /*  for (var result : camera.getAllUnreadResults()) {
-            visionEst = poseEstimator.estimateCoprocMultiTagPose(result);
-            
-            if (visionEst.isPresent()) {
-            EstimatedRobotPose estimatedPose = visionEst.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/Vision.java

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