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
@@ -82,13 +78,24 @@ public final class Constants {
         public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
         public static final int INDEXER_MOTOR_ID = 43;
         
-        public static double SHOOTER_MOTOR_P = 0.0018;
+        public static final int SHOOTER_MOTOR_CURRENT_LIMIT = 80;
-        public static double SHOOTER_MOTOR_I = 0;
-        public static double SHOOTER_MOTOR_D = 0;
 
-        public static double INDEXER_MOTOR_P = 0.0001;
+        public static final double SHOOTER_MOTOR_P = 0.0018;
-        public static double INDEXER_MOTOR_I = 0;
+        public static final double SHOOTER_MOTOR_I = 0;
-        public static double INDEXER_MOTOR_D = 0;
+        public static final double SHOOTER_MOTOR_D = 0;
+
+        public static final double INDEXER_MOTOR_P = 0.0001;
+        public static final double INDEXER_MOTOR_I = 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

@@ -13,11 +13,11 @@ public class ClimberSubsystem extends SubsystemBase{
     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() {
@@ -38,9 +38,9 @@ public class ClimberSubsystem extends SubsystemBase{
 
     public static void toggleRatchet(boolean toggle) {
       if (toggle == true) {
-      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
+        climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE_DEGREES);
       } else
-      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
+        climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE_DEGREES);
     }
 
   public Command toggleRatchetCommand(boolean 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);
-        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-                com.revrobotics.PersistMode.kNoPersistParameters);
-        intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
         
-        intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P,
+        intakeRotatorConfig
-                Constants.IntakeConstants.INTAKE_MOTOR_I,
+            .smartCurrentLimit(Constants.IntakeConstants.INTAKE_ROTATOR_CURRENT_LIMIT)
-                Constants.IntakeConstants.INTAKE_MOTOR_D);
+            .encoder
-        intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+            .positionConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR)
-                com.revrobotics.PersistMode.kNoPersistParameters);
+            .velocityConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR);
-        intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
+        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
+        intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_ROTATOR_INITIAL_ENCODER_VALUE);
+
+        intakeRotatorMotorPIDController = new PIDController(Constants.IntakeConstants.INTAKE_ROTATOR_UP_P, Constants.IntakeConstants.INTAKE_ROTATOR_UP_I, Constants.IntakeConstants.INTAKE_ROTATOR_UP_D);
+        intakeRotatorMotorFeedforward = new ArmFeedforward(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_S, Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_G, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
     }
 
     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);
-        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);
-        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);
-        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);
-        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/SwerveSubsystem.java

@@ -26,6 +26,7 @@ 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.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -41,7 +42,6 @@ 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;
@@ -53,7 +53,8 @@ import swervelib.parser.SwerveParser;
 import swervelib.telemetry.SwerveDriveTelemetry;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
-public class SwerveSubsystem extends SubsystemBase {
+public class SwerveSubsystem extends SubsystemBase
+{
   /**
    * Swerve drive object.
    */
@@ -62,7 +63,7 @@ public class SwerveSubsystem extends SubsystemBase {
   /**
    * Enable vision odometry updates while driving.
    */
-    private final boolean visionDriveTest = true;
+  private final boolean     visionDriveTest = false;
  
   /**
    * PhotonVision class to keep an accurate odometry.
@@ -74,46 +75,38 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @param directory Directory of swerve drive config files.
    */
-    public SwerveSubsystem(File directory) {
+   public SwerveSubsystem(File directory)
-        boolean blueAlliance = false;
+  { 
+    boolean blueAlliance = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Blue;
     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
+    // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being created.
-        // objects being created.
     SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
-        try {
+    try
-            swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose);
+    {
-            // Alternative method if you don't want to supply the conversion factor via JSON
+      swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED_MPS, startingPose);
-            // files.
+      // Alternative method if you don't want to supply the conversion factor via JSON files.
-            // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed,
+      // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor);
-            // angleConversionFactor, driveConversionFactor);
+    } catch (Exception e)
-        } catch (Exception e) {
+    {
       throw new RuntimeException(e);
     }
-        swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot
+    swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle.
-                                                 // via
+    swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life.
-                                                 // 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
+                                               0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1.
-                      // coefficient of 0.1.
     swerveDrive.setModuleEncoderAutoSynchronize(false,
-                1); // Enable if you want to resynchronize your absolute encoders and motor encoders
+                                                1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving.
-                    // 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
-        // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used
+    if (visionDriveTest)
-        // 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
+      // Stop the odometry thread if we are using vision that way we can synchronize updates better.
-            // updates better.
       swerveDrive.stopOdometryThread();
     }
     setupPathPlanner();
@@ -125,10 +118,11 @@ public class SwerveSubsystem extends SubsystemBase {
    * @param driveCfg      SwerveDriveConfiguration for the swerve.
    * @param controllerCfg Swerve Controller.
    */
-    public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) {
+  public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg)
+  {
     swerveDrive = new SwerveDrive(driveCfg,
                                   controllerCfg,
-                Constants.MAX_SPEED,
+                                  Constants.MAX_SPEED_MPS,
                                   new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)),
                                              Rotation2d.fromDegrees(0)));
   }
@@ -136,32 +130,37 @@ public class SwerveSubsystem extends SubsystemBase {
   /**
    * Setup the photon vision class.
    */
-    public void setupPhotonVision() {
+  public void setupPhotonVision()
+  {
     vision = new Vision(swerveDrive::getPose, swerveDrive.field);
   }
 
   @Override
-    public void periodic() {
+  public void periodic()
-
+  {
     // When vision is enabled we must manually update odometry in SwerveDrive
-        if (visionDriveTest) {
+    if (visionDriveTest)
+    {
       swerveDrive.updateOdometry();
-            //vision.updatePoseEstimation(swerveDrive);
+      vision.updatePoseEstimation(swerveDrive);
     }
   }
 
   @Override
-    public void simulationPeriodic() {
+  public void simulationPeriodic()
+  {
   }
 
   /**
    * Setup AutoBuilder for PathPlanner.
    */
-    public void setupPathPlanner() {
+  public void setupPathPlanner()
+  {
     // Load the RobotConfig from the GUI settings. You should probably
     // store this in your Constants file
     RobotConfig config;
-        try {
+    try
+    {
       config = RobotConfig.fromGUISettings();
 
       final boolean enableFeedforward = true;
@@ -174,35 +173,36 @@ public class SwerveSubsystem extends SubsystemBase {
           this::getRobotVelocity,
           // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
           (speedsRobotRelative, moduleFeedForwards) -> {
-                        if (enableFeedforward) {
+            if (enableFeedforward)
+            {
               swerveDrive.drive(
                   speedsRobotRelative,
                   swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative),
-                                    moduleFeedForwards.linearForces());
+                  moduleFeedForwards.linearForces()
-                        } else {
+                               );
+            } else
+            {
               swerveDrive.setChassisSpeeds(speedsRobotRelative);
             }
           },
-                    // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also
+          // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
-                    // optionally outputs individual module feedforwards
           new PPHolonomicDriveController(
-                            // PPHolonomicController is the built in path following controller for holonomic
+              // PPHolonomicController is the built in path following controller for holonomic drive trains
-                            // drive trains
               new PIDConstants(5.0, 0.0, 0.0),
               // Translation PID constants
-                            new PIDConstants(3.8, 0.0, 0.0)
+              new PIDConstants(5.0, 0.0, 0.0)
               // Rotation PID constants
           ),
           config,
           // The robot configuration
           () -> {
-                        // Boolean supplier that controls when the path will be mirrored for the red
+            // Boolean supplier that controls when the path will be mirrored for the red alliance
-                        // 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()) {
+            if (alliance.isPresent())
+            {
               return alliance.get() == DriverStation.Alliance.Red;
             }
             return false;
@@ -211,7 +211,8 @@ public class SwerveSubsystem extends SubsystemBase {
           // Reference to this subsystem to set requirements
                            );
 
-        } catch (Exception e) {
+    } catch (Exception e)
+    {
       // Handle exception as needed
       e.printStackTrace();
     }
@@ -226,13 +227,16 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return A {@link Command} which will run the alignment.
    */
-    public Command aimAtTarget(Cameras camera) {
+  public Command aimAtTarget(Cameras camera)
+  {
 
     return run(() -> {
       Optional<PhotonPipelineResult> resultO = camera.getBestResult();
-            if (resultO.isPresent()) {
+      if (resultO.isPresent())
+      {
         var result = resultO.get();
-                if (result.hasTargets()) {
+        if (result.hasTargets())
+        {
           drive(getTargetSpeeds(0,
                                 0,
                                 Rotation2d.fromDegrees(result.getBestTarget()
@@ -248,9 +252,9 @@ public class SwerveSubsystem extends SubsystemBase {
    * @param pathName PathPlanner path name.
    * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command.
    */
-    public Command getAutonomousCommand(String pathName) {
+  public Command getAutonomousCommand(String pathName)
-        // Create a path following command using AutoBuilder. This will also trigger
+  {
-        // event markers.
+    // Create a path following command using AutoBuilder. This will also trigger event markers.
     return new PathPlannerAuto(pathName);
   }
 
@@ -260,7 +264,8 @@ public class SwerveSubsystem extends SubsystemBase {
    * @param pose Target {@link Pose2d} to go to.
    * @return PathFinding command
    */
-    public Command driveToPose(Pose2d pose) {
+  public Command driveToPose(Pose2d pose)
+  {
 // Create the constraints to use while pathfinding
     PathConstraints constraints = new PathConstraints(
         swerveDrive.getMaximumChassisVelocity(), 4.0,
@@ -275,21 +280,20 @@ public class SwerveSubsystem extends SubsystemBase {
   }
 
   /**
-     * Drive with {@link SwerveSetpointGenerator} from 254, implemented by
+   * Drive with {@link SwerveSetpointGenerator} from 254, implemented by PathPlanner.
-     * PathPlanner.
    *
-     * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to
+   * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to achieve.
-     *                                  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 {
+  throws IOException, ParseException
+  {
     SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(),
                                                                             swerveDrive.getMaximumChassisAngularVelocity());
-        AtomicReference<SwerveSetpoint> prevSetpoint = new AtomicReference<>(
+    AtomicReference<SwerveSetpoint> prevSetpoint
-                new SwerveSetpoint(swerveDrive.getRobotVelocity(),
+        = new AtomicReference<>(new SwerveSetpoint(swerveDrive.getRobotVelocity(),
                                                    swerveDrive.getStates(),
                                                    DriveFeedforwards.zeros(swerveDrive.getModules().length)));
     AtomicReference<Double> previousTime = new AtomicReference<>();
@@ -315,25 +319,30 @@ public class SwerveSubsystem extends SubsystemBase {
    * @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds}
    * @return Command to drive the robot using the setpoint generator.
    */
-    public Command driveWithSetpointGeneratorFieldRelative(Supplier<ChassisSpeeds> fieldRelativeSpeeds) {
+  public Command driveWithSetpointGeneratorFieldRelative(Supplier<ChassisSpeeds> fieldRelativeSpeeds)
-        try {
+  {
+    try
+    {
       return driveWithSetpointGenerator(() -> {
         return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading());
 
       });
-        } catch (Exception e) {
+    } 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() {
+  public Command sysIdDriveMotorCommand()
+  {
     return SwerveDriveTest.generateSysIdCommand(
         SwerveDriveTest.setDriveSysIdRoutine(
             new Config(),
@@ -346,7 +355,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return SysId Angle Command
    */
-    public Command sysIdAngleMotorCommand() {
+  public Command sysIdAngleMotorCommand()
+  {
     return SwerveDriveTest.generateSysIdCommand(
         SwerveDriveTest.setAngleSysIdRoutine(
             new Config(),
@@ -359,50 +369,47 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return a Command that centers the modules of the SwerveDrive subsystem
    */
-    public Command centerModulesCommand() {
+  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
+   * Returns a Command that tells the robot to drive forward until the command ends.
-     * ends.
    *
-     * @return a Command that tells the robot to drive forward until the command
+   * @return a Command that tells the robot to drive forward until the command ends
-     *         ends
    */
-    public Command driveForward() {
+  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
+   * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object.
-     * 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) {
+  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
+   * Command to drive the robot using translative values and heading as angular velocity.
-     * velocity.
    *
-     * @param translationX     Translation in the X direction. Cubed for smoother
+   * @param translationX     Translation in the X direction. Cubed for smoother controls.
-     *                         controls.
+   * @param translationY     Translation in the Y direction. Cubed for smoother controls.
-     * @param translationY     Translation in the Y direction. Cubed for smoother
+   * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls.
-     *                         controls.
-     * @param angularRotationX Angular velocity of the robot to set. Cubed for
-     *                         smoother controls.
    * @return Drive command.
    */
-    public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY,
+  public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX)
-            DoubleSupplier angularRotationX) {
+  {
     return run(() -> {
       // Make the robot move
       swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d(
@@ -415,21 +422,18 @@ public class SwerveSubsystem extends SubsystemBase {
   }
 
   /**
-     * Command to drive the robot using translative values and heading as a
+   * Command to drive the robot using translative values and heading as a setpoint.
-     * setpoint.
    *
-     * @param translationX Translation in the X direction. Cubed for smoother
+   * @param translationX Translation in the X direction. Cubed for smoother controls.
-     *                     controls.
+   * @param translationY Translation in the Y 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) {
+                              DoubleSupplier headingY)
-        // swerveDrive.setHeadingCorrection(true); // Normally you would want heading
+  {
-        // correction for this kind of control.
+    // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control.
     return run(() -> {
 
       Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(),
@@ -445,29 +449,21 @@ public class SwerveSubsystem extends SubsystemBase {
   }
 
   /**
-     * The primary method for controlling the drivebase. Takes a
+   * The primary method for controlling the drivebase.  Takes a {@link Translation2d} and a rotation rate, and
-     * {@link Translation2d} and a rotation rate, and
+   * calculates and commands module states accordingly.  Can use either open-loop or closed-loop velocity control for
-     * calculates and commands module states accordingly. Can use either open-loop
+   * the wheel velocities.  Also has field- and robot-relative modes, which affect how the translation vector is used.
-     * 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
+   * @param translation   {@link Translation2d} that is the commanded linear velocity of the robot, in meters per
-     *                      velocity of the robot, in meters per
+   *                      second. In robot-relative mode, positive x is torwards the bow (front) and positive y is
-     *                      second. In robot-relative mode, positive x is torwards
+   *                      torwards port (left).  In field-relative mode, positive x is away from the alliance wall
-     *                      the bow (front) and positive y is
+   *                      (field North) and positive y is torwards the left wall when looking through the driver station
-     *                      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.
+   * @param rotation      Robot angular rate, in radians per second. CCW positive.  Unaffected by field/robot
-     *                      Unaffected by field/robot
    *                      relativity.
-     * @param fieldRelative Drive mode. True for field-relative, false for
+   * @param fieldRelative Drive mode.  True for field-relative, false for robot-relative.
-     *                      robot-relative.
    */
-    public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
+  public void drive(Translation2d translation, double rotation, boolean fieldRelative)
+  {
     swerveDrive.drive(translation,
                       rotation,
                       fieldRelative,
@@ -479,7 +475,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @param velocity Velocity according to the field.
    */
-    public void driveFieldOriented(ChassisSpeeds velocity) {
+  public void driveFieldOriented(ChassisSpeeds velocity)
+  {
     swerveDrive.driveFieldOriented(velocity);
   }
 
@@ -488,7 +485,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @param velocity Velocity according to the field.
    */
-    public Command driveFieldOriented(Supplier<ChassisSpeeds> velocity) {
+  public Command driveFieldOriented(Supplier<ChassisSpeeds> velocity)
+  {
     return run(() -> {
       swerveDrive.driveFieldOriented(velocity.get());
     });
@@ -499,7 +497,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @param velocity Robot oriented {@link ChassisSpeeds}
    */
-    public void drive(ChassisSpeeds velocity) {
+  public void drive(ChassisSpeeds velocity)
+  {
     swerveDrive.drive(velocity);
   }
 
@@ -508,30 +507,30 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return {@link SwerveDriveKinematics} of the swerve drive.
    */
-    public SwerveDriveKinematics getKinematics() {
+  public SwerveDriveKinematics getKinematics()
+  {
     return swerveDrive.kinematics;
   }
 
   /**
-     * Resets odometry to the given pose. Gyro angle and module positions do not
+   * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this
-     * 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
-     * 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) {
+  public void resetOdometry(Pose2d initialHolonomicPose)
+  {
     swerveDrive.resetOdometry(initialHolonomicPose);
   }
 
   /**
-     * Gets the current pose (position and rotation) of the robot, as reported by
+   * Gets the current pose (position and rotation) of the robot, as reported by odometry.
-     * odometry.
    *
    * @return The robot's pose
    */
-    public Pose2d getPose() {
+  public Pose2d getPose()
+  {
     return swerveDrive.getPose();
   }
 
@@ -540,7 +539,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @param chassisSpeeds Chassis Speeds to set.
    */
-    public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
+  public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
+  {
     swerveDrive.setChassisSpeeds(chassisSpeeds);
   }
 
@@ -549,41 +549,44 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @param trajectory The trajectory to post.
    */
-    public void postTrajectory(Trajectory trajectory) {
+  public void postTrajectory(Trajectory trajectory)
+  {
     swerveDrive.postTrajectory(trajectory);
   }
 
   /**
-     * Resets the gyro angle to zero and resets odometry to the same position, but
+   * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
-     * facing toward 0.
    */
-    public void zeroGyro() {
+  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
+   * @return true if the red alliance, false if blue. Defaults to false if none is available.
-     *         available.
    */
-    private boolean isRedAlliance() {
+  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
+   * This will zero (calibrate) the robot to assume the current position is facing forward
-     * forward
    * <p>
    * If red alliance rotate the robot 180 after the drviebase zero command
    */
-    public void zeroGyroWithAlliance() {
+  public void zeroGyroWithAlliance()
-        if (isRedAlliance()) {
+  {
+    if (isRedAlliance())
+    {
       zeroGyro();
       //Set the pose 180 degrees
       resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180)));
-        } else {
+    } else
+    {
       zeroGyro();
     }
   }
@@ -593,25 +596,24 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @param brake True to set motors to brake mode, false for coast.
    */
-    public void setMotorBrake(boolean brake) {
+  public void setMotorBrake(boolean brake)
+  {
     swerveDrive.setMotorIdleMode(brake);
   }
 
   /**
-     * Gets the current yaw angle of the robot, as reported by the swerve pose
+   * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the underlying drivebase.
-     * estimator in the underlying drivebase.
+   * Note, this is not the raw gyro reading, this may be corrected from calls to resetOdometry().
-     * Note, this is not the raw gyro reading, this may be corrected from calls to
-     * resetOdometry().
    *
    * @return The yaw angle
    */
-    public Rotation2d getHeading() {
+  public Rotation2d getHeading()
+  {
     return getPose().getRotation();
   }
 
   /**
-     * Get the chassis speeds based on controller input of 2 joysticks. One for
+   * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other 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.
@@ -620,19 +622,19 @@ public class SwerveSubsystem extends SubsystemBase {
    * @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) {
+  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);
+                                                        Constants.MAX_SPEED_MPS);
   }
 
   /**
-     * Get the chassis speeds based on controller input of 1 joystick and one angle.
+   * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of
-     * Control the robot at an offset of
    * 90deg.
    *
    * @param xInput X joystick input for the robot to move in the X direction.
@@ -640,14 +642,15 @@ public class SwerveSubsystem extends SubsystemBase {
    * @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) {
+  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);
+                                                        Constants.MAX_SPEED_MPS);
   }
 
   /**
@@ -655,7 +658,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return A ChassisSpeeds object of the current field-relative velocity
    */
-    public ChassisSpeeds getFieldVelocity() {
+  public ChassisSpeeds getFieldVelocity()
+  {
     return swerveDrive.getFieldVelocity();
   }
 
@@ -664,7 +668,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return A {@link ChassisSpeeds} object of the current velocity
    */
-    public ChassisSpeeds getRobotVelocity() {
+  public ChassisSpeeds getRobotVelocity()
+  {
     return swerveDrive.getRobotVelocity();
   }
 
@@ -673,7 +678,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return {@link SwerveController} from the {@link SwerveDrive}.
    */
-    public SwerveController getSwerveController() {
+  public SwerveController getSwerveController()
+  {
     return swerveDrive.swerveController;
   }
 
@@ -682,14 +688,16 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return The {@link SwerveDriveConfiguration} fpr the current drive.
    */
-    public SwerveDriveConfiguration getSwerveDriveConfiguration() {
+  public SwerveDriveConfiguration getSwerveDriveConfiguration()
+  {
     return swerveDrive.swerveDriveConfiguration;
   }
 
   /**
    * Lock the swerve drive to prevent it from moving.
    */
-    public void lock() {
+  public void lock()
+  {
     swerveDrive.lockPose();
   }
 
@@ -698,14 +706,16 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return The heading as a {@link Rotation2d} angle
    */
-    public Rotation2d getPitch() {
+  public Rotation2d getPitch()
+  {
     return swerveDrive.getPitch();
   }
 
   /**
    * Add a fake vision reading for testing purposes.
    */
-    public void addFakeVisionReading() {
+  public void addFakeVisionReading()
+  {
     swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp());
   }
 
@@ -714,7 +724,8 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @return {@link SwerveDrive}
    */
-    public SwerveDrive getSwerveDrive() {
+  public SwerveDrive getSwerveDrive()
+  {
     return swerveDrive;
   }
 }

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,21 +40,21 @@ 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(
-      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;
   /**
@@ -62,8 +62,7 @@ public class Vision {
    */
   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;
   /**
@@ -75,22 +74,25 @@ public class Vision {
    */
   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,44 +104,46 @@ 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,
@@ -156,16 +160,18 @@ public class Vision {
    * <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 ->
+              debugField
                   .getObject("VisionEstimation")
                   .setPose(est.estimatedPose.toPose2d()),
           () -> {
@@ -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
-      if (bestTargetAmbiguity > maximumAmbiguity) {
+      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) {
+      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,16 +273,18 @@ 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)) {
+  {
+    if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE))
+    {
 //      try
 //      {
 //        Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
@@ -278,21 +300,27 @@ public class Vision {
   /**
    * 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,34 +332,38 @@ 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.
@@ -354,8 +386,7 @@ public class Vision {
      */
     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;
     /**
@@ -372,8 +403,7 @@ public class Vision {
      */
     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<>();
     /**
@@ -382,22 +412,18 @@ public class Vision {
     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);
@@ -413,15 +439,14 @@ public class Vision {
       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);
       double               amiguity         = bestResult.getBestTarget().getPoseAmbiguity();
       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,33 +501,34 @@ 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());
       }
 
@@ -506,29 +536,28 @@ public class Vision {
         resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
           return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
         });
-      if (!resultsList.isEmpty()) {
+        if (!resultsList.isEmpty())
+        {
           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.
      */
-    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;
         int    numTags    = 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 +=
+              tagPose
                   .get()
                   .toPose2d()
                   .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;