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

.gitignore

@@ -0,0 +1,187 @@
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
+.classpath
+.project
+.settings/
+bin/
+
+# IntelliJ
+*.iml
+*.ipr
+*.iws
+.idea/
+out/
+
+# Fleet
+.fleet
+
+# Simulation GUI and other tools window save file
+networktables.json
+simgui.json
+*-window.json
+
+# Simulation data log directory
+logs/
+
+# Folder that has CTRE Phoenix Sim device config storage
+ctre_sim/
+
+# clangd
+/.cache
+compile_commands.json
+
+# Eclipse generated file for annotation processors
+.factorypath

.vscode/launch.json

@@ -0,0 +1,21 @@
+{
+  // Use IntelliSense to learn about possible attributes.
+  // Hover to view descriptions of existing attributes.
+  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+  "version": "0.2.0",
+  "configurations": [
+
+    {
+      "type": "wpilib",
+      "name": "WPILib Desktop Debug",
+      "request": "launch",
+      "desktop": true,
+    },
+    {
+      "type": "wpilib",
+      "name": "WPILib roboRIO Debug",
+      "request": "launch",
+      "desktop": false,
+    }
+  ]
+}

.vscode/settings.json

@@ -0,0 +1,61 @@
+{
+  "java.configuration.updateBuildConfiguration": "automatic",
+  "java.server.launchMode": "Standard",
+  "files.exclude": {
+    "**/.git": true,
+    "**/.svn": true,
+    "**/.hg": true,
+    "**/CVS": true,
+    "**/.DS_Store": true,
+    "bin/": true,
+    "**/.classpath": true,
+    "**/.project": true,
+    "**/.settings": true,
+    "**/.factorypath": true,
+    "**/*~": true
+  },
+  "java.test.config": [
+    {
+      "name": "WPIlibUnitTests",
+      "workingDirectory": "${workspaceFolder}/build/jni/release",
+      "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
+      "env": {
+        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
+        "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
+      }
+    },
+  ],
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.import.gradle.annotationProcessing.enabled": false,
+  "java.completion.favoriteStaticMembers": [
+    "org.junit.Assert.*",
+    "org.junit.Assume.*",
+    "org.junit.jupiter.api.Assertions.*",
+    "org.junit.jupiter.api.Assumptions.*",
+    "org.junit.jupiter.api.DynamicContainer.*",
+    "org.junit.jupiter.api.DynamicTest.*",
+    "org.mockito.Mockito.*",
+    "org.mockito.ArgumentMatchers.*",
+    "org.mockito.Answers.*",
+    "edu.wpi.first.units.Units.*"
+  ],
+  "java.completion.filteredTypes": [
+    "java.awt.*",
+    "com.sun.*",
+    "sun.*",
+    "jdk.*",
+    "org.graalvm.*",
+    "io.micrometer.shaded.*",
+    "java.beans.*",
+    "java.util.Base64.*",
+    "java.util.Timer",
+    "java.sql.*",
+    "javax.swing.*",
+    "javax.management.*",
+    "javax.smartcardio.*",
+    "edu.wpi.first.math.proto.*",
+    "edu.wpi.first.math.**.proto.*",
+    "edu.wpi.first.math.**.struct.*",
+  ],
+  "java.dependency.enableDependencyCheckup": false
+}

.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2026",
+    "teamNumber": 2890
+}

src/main/deploy/pathplanner/paths/test_1_Meter.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.075,
+        "x": 2.0,
-        "y": 4.75
+        "y": 7.0
       },
       "prevControl": {
-        "x": 2.380178890876563,
+        "x": 0.8572206587126847,
-        "y": 5.341144901610018
+        "y": 7.0
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -90.0
+    "rotation": 0.0
   },
   "reversed": false,
   "folder": null,

src/main/deploy/swerve/neo/controllerproperties.json

@@ -2,7 +2,7 @@
   "angleJoystickRadiusDeadband": 0.5,
   "heading": {
     "p": 0.4,
-    "i": 0,
+    "i": 0.01,
     "d": 0.01
   }
 }

src/main/deploy/swerve/neo/modules/backleft.json

@@ -3,7 +3,7 @@
     "front": -10.9,
     "left": 10.9
   },
-  "absoluteEncoderOffset": 216.38671875,
+  "absoluteEncoderOffset": 215.947265625,
   "drive": {
     "type": "sparkflex_neo",
     "id": 12,

src/main/deploy/swerve/neo/modules/backright.json

@@ -3,7 +3,7 @@
     "front": -10.9,
     "left": -10.9
   },
-  "absoluteEncoderOffset": 191.6015625,
+  "absoluteEncoderOffset": 287.578125,
   "drive": {
     "type": "sparkflex_neo",
     "id": 11,

src/main/deploy/swerve/neo/modules/frontleft.json

@@ -3,7 +3,7 @@
     "front": 10.9,
     "left": 10.9
   },
-  "absoluteEncoderOffset": 198.896484375,
+  "absoluteEncoderOffset": 36.03515625,
   "drive": {
     "type": "sparkflex_neo",
     "id": 13,

src/main/deploy/swerve/neo/modules/frontright.json

@@ -3,7 +3,7 @@
     "front": 10.9,
     "left": -10.9
   },
-  "absoluteEncoderOffset": 231.064453125,
+  "absoluteEncoderOffset": 318.779296875,
   "drive": {
     "type": "sparkflex_neo",
     "id": 10,

src/main/deploy/swerve/neo/modules/pidfproperties.json

@@ -1,16 +1,16 @@
 {
   "drive": {
-    "p": 0.001,
+    "p": 0.15,
     "i": 0,
     "d": 0,
-    "f": 0,
+    "f": 0.15,
     "iz": 0
   },
   "angle": {
-    "p": 0.002,
+    "p": 0.008,
-    "i": 0.0,
+    "i": 0.00001,
-    "d": 0.0,
+    "d": 0,
-    "f": 0,
+    "f": 0.2,
     "iz": 0
   }
 }

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

@@ -4,12 +4,13 @@
 
 package frc.robot;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 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;
@@ -36,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
@@ -63,103 +64,129 @@ public final class Constants {
     }
 
     public static class ShooterConstants {
-        private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter Velocity", -0.5)
+        private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000)
                 .withWidget(BuiltInWidgets.kNumberBar).getEntry();
 
-        public static double SHOOTER_VELOCITY = -0.6;
+        public static double SHOOTER_RPM;
-        public static double SHOOTER_POWER = -0.45;
 
-        public static void getShooterVelocity() {
+        public static void updateShooterRPM() {
-            SHOOTER_VELOCITY = shooterVelocity.getDouble(-0.5);
+            SHOOTER_RPM = shooterRPM.getDouble(-4000);
         }
 
         public static final int CENTER_SHOOTER_MOTOR_ID = 42;
         public static final int LEFT_SHOOTER_MOTOR_ID = 41;
         public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
         public static final int INDEXER_MOTOR_ID = 43;
-        public static double INDEXER_MOTOR_SPEED = 0.6;
         
-        private static GenericEntry indexerAndRampSpeed = programmingTab.add("Desired Ramp + Indexer Speed", 0.6)
+        public static final int SHOOTER_MOTOR_CURRENT_LIMIT = 80;
-                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+
+        public static final double SHOOTER_MOTOR_P = 0.0018;
+        public static final double SHOOTER_MOTOR_I = 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)
+                .withWidget(BuiltInWidgets.kNumberBar).getEntry();*/
+
+
+        public static double INDEXER_AND_RAMP_MOTOR_RPM = 10000;
 
         // this method called in robot periodic so values updated in elastic are
         // constantly read and applied to RAMP_MOTOR_SPEED
-        public static void getRampMotorSpeed() {
+        /*public static void updateIndexerAndRampMotorRPM() {
-            INDEXER_MOTOR_SPEED = indexerAndRampSpeed.getDouble(.6);
+            INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000);
-        }
+        }*/
-
-        public static final int LEFT_ACTUATOR_PWM_PORT = 1;
-        public static final int RIGHT_ACTUATOR_PWM_PORT = 3;
-
-        public static double DESIRED_POTENTIOMETER_DISTANCE;
-
-        // TO DO: need to equation that calculates desired potentiometer distance
     }
 
     public static class IntakeConstants {
-        private static GenericEntry intakeSpeed = programmingTab.add("Desired Intake Speed", -0.4)
-                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
 
-        public static double INTAKE_WHEELS_MOTOR_SPEED;
+        /* private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000)
+                .withWidget(BuiltInWidgets.kNumberBar).getEntry(); */
+        public static double INTAKE_WHEELS_MOTOR_RPM = -7000;
 
-        public static void getIntakeWheelsSpeed() {
+        /*public static void updateIntakeWheelsRPM() {
-            INTAKE_WHEELS_MOTOR_SPEED = intakeSpeed.getDouble(-0.4);
+            INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000);
-        }
+        }*/
 
         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 double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535;
+        public static final int INTAKE_ROTATOR_CURRENT_LIMIT = 40;
-        public static final double INTAKE_MIDDLE_ENCODER_VALUE = 2.2550222873687744;
+        public static final double INTAKE_ROTATOR_INITIAL_ENCODER_VALUE = Units.degreesToRadians(-90);
-        public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
+        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.05;
+        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.001;
+        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_ROTATOR_DOWN_P = 0.03; // Radians -> Radians Per Second
+        public static final double INTAKE_ROTATOR_DOWN_I = 0.0; // Radians -> Radians Per Second
+        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);
     }
 
-    }
-
-    public static class RampConstants {
-        public static final int RAMP_MOTOR_ID = 45;
-        public static double RAMP_MOTOR_SPEED = .6;
 
         // create object and a new widget under programming tab in Elastic where object
         // retrieves value from widget
 
-    }
 
-    public static class LimeLight {
-
-        public static final String LIMELIGHT_NAME = "limelight";
-
-        // public static final int[] ALL_REEF_APRILTAGS = { 6, 7, 8, 9, 10, 11, 17, 18,
-        // 19, 20, 21, 22 };
-
-        public static final AprilTagFieldLayout APRILTAG_FIELD_LAYOUT = AprilTagFieldLayout
-                .loadField(AprilTagFields.k2026RebuiltAndymark);
-
-        public static final double BUMPER_WIDTH = Units.inchesToMeters(0.0); // Get This Value // Original: 2.75
-        // public static final double ROBOT_WIDTH = Units.inchesToMeters(30 +
-        // BUMPER_WIDTH); // Tis a square, don't need this
-        public static final double ROBOT_SIDE_LENGTH = Units.inchesToMeters(27);
-        // public static final Transform2d HALF_ROBOT = new
-        // Transform2d(ROBOT_SIDE_LENGTH / 3.0, 0, new Rotation2d());
-
-        public static double LIMELIGHT_TY;
-    }
 
     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 Vector<N3> SINGLE_TAG_STD_DEVS = VecBuilder.fill(4, 4, 8);
+        public static final Vector<N3> MULTI_TAG_STD_DEVS = VecBuilder.fill(0.5, 0.5, 1);
+
+        public static final Pose2d HUB_POSE_METERS = new Pose2d(4.625, 4.03, new Rotation2d());
+
+        public static final Translation3d FRONT_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
+        public static final Translation3d FRONT_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
+        public static final Translation3d REAR_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
+        public static final Translation3d REAR_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
+
+        public static final Rotation3d FRONT_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
+        public static final Rotation3d FRONT_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
+        public static final Rotation3d REAR_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
+        public static final Rotation3d REAR_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
     }
 
     public static class ClimberConstants {
         public static final int CLIMB_MOTOR_ID = 60;
-        public static final int RATCHET_PWM_PORT = 0;
+        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,20 +4,11 @@
 
 package frc.robot;
 
-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
@@ -33,8 +24,7 @@ public class Robot extends TimedRobot {
     private RobotContainer m_robotContainer;
 
     private Timer disabledTimer;
-    private static NetworkTable table;
+
-    private static GenericEntry distanceFromLimelight;
 
     public Robot() {
         instance = this;
@@ -64,8 +54,7 @@ public class Robot extends TimedRobot {
             DriverStation.silenceJoystickConnectionWarning(true);
         }
 
-        table = NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
+        m_robotContainer.getSwerveDrive().zeroGyroWithAlliance();
-        distanceFromLimelight = Shuffleboard.getTab("Programming").add("Distance from Limelight", 0).getEntry();
     }
 
     /**
@@ -90,10 +79,9 @@ public class Robot extends TimedRobot {
         CommandScheduler.getInstance().run();
 
         //Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
-        Constants.IntakeConstants.getIntakeWheelsSpeed();
+        //Constants.IntakeConstants.updateIntakeWheelsRPM();
-        Constants.ShooterConstants.getShooterVelocity();
+        Constants.ShooterConstants.updateShooterRPM();
-        Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
+        //Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
-        //distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
     }
 
     /**

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,10 +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 frc.robot.subsystems.TargetingSubsystems;
 import swervelib.SwerveInputStream;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.ClimberSubsystem;
@@ -50,8 +44,10 @@ public class RobotContainer {
 
     // Replace with CommandPS4Controller or CommandJoystick if needed
     final CommandXboxController driverXbox = new CommandXboxController(0);
+    final CommandXboxController operatorXbox = new CommandXboxController(1);
+
     // The robot's subsystems and commands are defined here...
-    private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
+    private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
             "swerve/neo"));
 
     // Establish a Sendable Chooser that will be able to be sent to the
@@ -80,7 +76,8 @@ public class RobotContainer {
      * Clone's the angular velocity input stream and converts it to a fieldRelative
      * input stream.
      */
-    SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX,
+    SwerveInputStream driveDirectAngle = driveAngularVelocity.copy()
+            .withControllerHeadingAxis(driverXbox::getRightX,
                     driverXbox::getRightY)
             .headingWhile(true);
 
@@ -166,44 +163,49 @@ public class RobotContainer {
         Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
                 driveDirectAngle);
         Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
-        Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard);
+        Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
+                .driveFieldOriented(driveAngularVelocityKeyboard);
         Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
                 driveDirectAngleKeyboard);
 
-        driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand())
+        driverXbox.leftTrigger().whileTrue(m_IntakeSubsystem.startIntakeMotorCommand())
                 .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
-        driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand())
+        driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand().andThen(m_ShooterSubsystem.reverseIndexerAndRampMotorRPMCommand()))
-                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
+                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand().andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand()));
-        
         // command for
         // full shooting system including linear actuators
-        driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()
+        driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
-                .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
+               // .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
 
         driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
 
-        driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
+        driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand())
-        driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
+                .onFalse(m_ClimberSubsystem.stopClimberCommand());
+        driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand())
+                .onFalse(m_ClimberSubsystem.stopClimberCommand());
         driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
         driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
         driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
         driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
-
+        driverXbox.rightStick().onTrue(climbCommand());
 
         // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
-        driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand().andThen(m_IntakeSubsystem.deployintakeCommand()));
+        driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand()
+                .andThen(m_IntakeSubsystem.deployintakeCommand()));
         // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
         // () -> -driverXbox.getLeftX()));
-        if (driverXbox.getRightY() < -0.4) {
-            m_ClimberSubsystem.liftRobotCommand();
-        } else if (driverXbox.getRightY() > 0.4) {
-            m_ClimberSubsystem.lowerRobotCommand();
-        } else {
-            m_ClimberSubsystem.stopClimberCommand();
-        }
 
         // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
 
+        operatorXbox.x().whileTrue(m_ShooterSubsystem.testLeftShooterCommand())
+                .onFalse(m_ShooterSubsystem.stopLeftShooterCommand());
+        operatorXbox.y().whileTrue(m_ShooterSubsystem.testCenterShooterCommand())
+                .onFalse(m_ShooterSubsystem.stopCenterShooterCommand());
+        operatorXbox.b().whileTrue(m_ShooterSubsystem.testRightShooterCommand())
+                .onFalse(m_ShooterSubsystem.stopRightShooterCommand());
+        operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
+                .onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
+
         if (RobotBase.isSimulation()) {
             drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
         } else {
@@ -225,9 +227,12 @@ public class RobotContainer {
                             new Constraints(Units.degreesToRadians(360),
                                     Units.degreesToRadians(180))));
             driverXbox.start()
-                    .onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
+                    .onTrue(Commands.runOnce(() -> drivebase
+                            .resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
             driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
-            driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
+            driverXbox.button(2)
+                    .whileTrue(Commands.runEnd(
+                            () -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
                             () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
 
             // driverXbox.b().whileTrue(
@@ -237,7 +242,8 @@ public class RobotContainer {
 
         }
         if (DriverStation.isTest()) {
-            drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above!
+            drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
+                                                                             // above!
 
             driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
             driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
@@ -270,39 +276,22 @@ public class RobotContainer {
         drivebase.setMotorBrake(brake);
     }
 
-    public Command aimAtHopperCommand(DoubleSupplier xSup, DoubleSupplier ySup) {
+    public SwerveSubsystem getSwerveDrive() {
-        try (PIDController aimPIDs = new PIDController(0.3, 0, 0.001)) {
-            aimPIDs.setTolerance(1.0);
-
-            return Commands.run(() -> {
-
-                double xSpeed = xSup.getAsDouble();
-                double ySpeed = ySup.getAsDouble();
-
-                double rot = 0.0;
-
-                if (LimelightHelpers.getTV("limelight")) {
-                    double tx = LimelightHelpers.getTX("limelight");
-                    rot = aimPIDs.calculate(tx, 0);
-                    rot = MathUtil.clamp(rot, -1.5, 1.5);
-                }
-
-                drivebase.drive(new Translation2d(xSpeed, ySpeed), rot, false);
-            },
-                    drivebase);
-        }
-    }
-
-    public SwerveSubsystem getSwerveDriveBase() {
         return drivebase;
     }
 
-    public CommandXboxController getDriverXbox() {
+    public Command climbCommand() {
-        return driverXbox;
+        if (driverXbox.getRightY() > -0.5) {
+            return m_ClimberSubsystem.lowerRobotCommand();
+        } else if (driverXbox.getRightX() < 0.5) {
+            return m_ClimberSubsystem.liftRobotCommand();
+        } else
+            return m_ClimberSubsystem.stopClimberCommand();
+
     }
 
     public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
             // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
-            m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
+            m_ShooterSubsystem.shootFuelCommand(),
-
+            m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
 }

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) {
@@ -50,6 +50,6 @@ public class ClimberSubsystem extends SubsystemBase{
   @Override
   public void periodic()
   {
-    SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition());
+    SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getAngle());
   }
 }

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

@@ -1,51 +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 intakeMotor = 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, MotorType.kBrushless);
+
+    public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
+    private static BangBangController intakeWheelsMotorBBController = new BangBangController();
+    private static SimpleMotorFeedforward intakeWheelsMotorFeedforward;
 
-    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
-            MotorType.kBrushless);
-    private static SparkClosedLoopController intakeRotatorPIDController;
     public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
+    private static PIDController intakeRotatorMotorPIDController;
+    private static ArmFeedforward intakeRotatorMotorFeedforward;
 
     public IntakeSubsystem() {
-        intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
+        intakeWheelsMotorConfig
-                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
+            .smartCurrentLimit(Constants.IntakeConstants.INTAKE_WHEELS_CURRENT_LIMIT)
-                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D);
+            .encoder
-        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+            .positionConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_POSITION_CONVERSION_FACTOR)
-                com.revrobotics.PersistMode.kNoPersistParameters);
+            .velocityConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR);
-        intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
+        intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
+
+        intakeWheelsMotorFeedforward = new SimpleMotorFeedforward(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_S, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
+        
+        intakeRotatorConfig
+            .smartCurrentLimit(Constants.IntakeConstants.INTAKE_ROTATOR_CURRENT_LIMIT)
+            .encoder
+            .positionConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR)
+            .velocityConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR);
+        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
+        intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_ROTATOR_INITIAL_ENCODER_VALUE);
+
+        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() {
-        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED);
+        intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM);
     }
 
     public void reverseIntakeMotor() {
-        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED * -1);
+        intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1);
     }
 
     public void stopIntakeMotor() {
-        intakeMotor.set(0);
+        intakeWheelsMotorBBController.setSetpoint(0.0);
     }
 
     public Command startIntakeMotorCommand() {
@@ -61,8 +75,7 @@ public class IntakeSubsystem extends SubsystemBase {
     }
 
     public void deployIntake() {
-        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
+        intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_POSITION_RADS);
-                ControlType.kPosition);
     }
 
     public Command deployintakeCommand() {
@@ -70,7 +83,7 @@ public class IntakeSubsystem extends SubsystemBase {
     }
 
     public void retractIntake() {
-        intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition);
+        intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_POSITION_RADS);
     }
 
     public Command retractIntakeCommand() {
@@ -78,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() {
@@ -89,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,29 +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.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,
@@ -35,9 +23,47 @@ public class ShooterSubsystem extends SubsystemBase {
     private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
             MotorType.kBrushless);
 
-    private static SparkFlex indexerMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
+    private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
             MotorType.kBrushless);
 
+    public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig();
+    private static BangBangController centerShooterMotorBBController = new BangBangController();
+    private static SimpleMotorFeedforward centerShooterMotorFeedforward;
+
+    public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
+    private static BangBangController leftShooterMotorBBController = new BangBangController();
+    private static SimpleMotorFeedforward leftShooterMotorFeedforward;
+
+    public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig();
+    private static BangBangController rightShooterMotorBBController = new BangBangController();
+    private static SimpleMotorFeedforward rightShooterMotorFeedforward;
+
+    public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig();
+    private static PIDController indexerAndRampMotorPIDController;
+
+    public ShooterSubsystem() {
+        centerShooterMotorConfig.smartCurrentLimit(60);
+        centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+            com.revrobotics.PersistMode.kNoPersistParameters);
+        centerShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.CENTER_MOTOR_S, Constants.ShooterConstants.CENTER_MOTOR_V);
+    
+        leftShooterMotorConfig.smartCurrentLimit(60);
+        leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+            com.revrobotics.PersistMode.kNoPersistParameters);
+        leftShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.LEFT_MOTOR_S, Constants.ShooterConstants.LEFT_MOTOR_V);
+    
+        rightShooterMotorConfig.smartCurrentLimit(60);
+        rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+            com.revrobotics.PersistMode.kNoPersistParameters);
+        rightShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.RIGHT_MOTOR_S, Constants.ShooterConstants.RIGHT_MOTOR_V);
+
+        indexerAndRampMotorConfig.smartCurrentLimit(60);
+        indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+            com.revrobotics.PersistMode.kNoPersistParameters);
+        indexerAndRampMotorPIDController = new PIDController(Constants.ShooterConstants.INDEXER_MOTOR_P, Constants.ShooterConstants.INDEXER_MOTOR_I, Constants.ShooterConstants.INDEXER_MOTOR_D);
+    }
+
+
     //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
       //      MotorType.kBrushless);
 
@@ -49,44 +75,98 @@ public class ShooterSubsystem extends SubsystemBase {
 
     private static double currentPotentiometerPosition; // might need second value for the right potentiometer
 
-    public void startShooterMotors() {
+    public void setShooterMotorsRPM() {
-        centerShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-        leftShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-        rightShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
     }
 
-    public double getShooterMotorVelocity() {
+    //test individual motor code
-        return leftShooterMotor.getEncoder().getVelocity();
+    public void setLeftShooterMotorRPM() {
+        leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
+    }
+    public Command testLeftShooterCommand() {
+        return runOnce(() -> setLeftShooterMotorRPM());
     }
 
-    public void startIndexerMotor() {
+    public void setRightShooterMotorRPM() {
-        // if (LimelightHelpers.getTX("limelight") < 1.5 &&
+        rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-        // LimelightHelpers.getTX("limelight") > -1.5) {
+    }
-        indexerMotor.set(Constants.ShooterConstants.INDEXER_MOTOR_SPEED);
+    public Command testRightShooterCommand() {
-        // } else
+        return runOnce(() -> setRightShooterMotorRPM());
-        // indexerMotor.set(0);
+    }
+
+    public void setCenterShooterMotorRPM() {
+        centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
+    }
+    public Command testCenterShooterCommand() {
+        return runOnce(() -> setCenterShooterMotorRPM());
+    }
+
+    public void stopLeftShooterMotorRPM() {
+        leftShooterMotorBBController.setSetpoint(0.0);
+    }
+    public Command stopLeftShooterCommand() {
+        return runOnce(() -> stopLeftShooterMotorRPM());
+    }
+
+    public void stopCenterShooterMotorRPM() {
+        centerShooterMotorBBController.setSetpoint(0.0);
+    }
+    public Command stopCenterShooterCommand() {
+        return runOnce(() -> stopCenterShooterMotorRPM());
+    }
+
+    public void stopRightShooterMotorRPM() {
+        rightShooterMotorBBController.setSetpoint(0.0);
+    }
+    public Command stopRightShooterCommand() {
+        return runOnce(() -> stopRightShooterMotorRPM());
+    }
+    
+
+    public double getShooterMotorRPM() {
+        return rightShooterMotor.getEncoder().getVelocity();
+    }
+
+    public void setIndexerAndRampMotorRPM() {
+        indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM);
+    }
+
+    public void reverseIndexerAndRampMotorRPM() {
+        indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1);
+    }
+
+    public Command reverseIndexerAndRampMotorRPMCommand() {
+        return runOnce(() -> reverseIndexerAndRampMotorRPM());
+    }
+
+    public Command setIndexerAndRampMotorRPMCommand() {
+        return runOnce(() -> setIndexerAndRampMotorRPM());
+    }
+
+    public Command stopIndexerAndRampMotorCommand() {
+        return runOnce(()-> indexerAndRampMotorPIDController.setSetpoint(0.0));
     }
 
     /*public Command shootFuelCommand() {
-        return run(() -> startShooterMotors())
+        return runOnce(() -> setShooterMotorsRPM())
-                .until(() -> {
+                .until(() -> {return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM * 0.9);})
-                    return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY);
+                .andThen(() -> setIndexerAndRampMotorRPM()); 
-                })
-                .andThen(() -> startIndexerMotor());
     } */
 
     
       public Command shootFuelCommand() {
-      return runOnce(() -> startShooterMotors()).andThen(new WaitCommand(2))
+      return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
-      .andThen(() -> startIndexerMotor());
+      .andThen(() -> setIndexerAndRampMotorRPM());
       }; 
      
 
     public void stopShooters() {
-        centerShooterMotor.set(0);
+        centerShooterMotorBBController.setSetpoint(0.0);
-        leftShooterMotor.set(0);
+        leftShooterMotorBBController.setSetpoint(0.0);
-        rightShooterMotor.set(0);
+        rightShooterMotorBBController.setSetpoint(0.0);
-        indexerMotor.set(0);
+        indexerAndRampMotorPIDController.setSetpoint(0.0);
     }
 
     public Command stopShooterCommand() {
@@ -117,6 +197,19 @@ 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());
         SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
         currentPotentiometerPosition = leftPotentiometer.get(); */

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

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

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

@@ -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.
    */
@@ -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);
     }
   }
 
   @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;
@@ -52,7 +52,7 @@ 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 {@link Vision#filterPose}.
    */
@@ -337,30 +337,33 @@ public class Vision
     /**
      * 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.

vendordeps/REVLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2026.0.2",
+    "version": "2026.0.3",
     "frcYear": "2026",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2026.0.2"
+            "version": "2026.0.3"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -34,7 +34,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -49,7 +49,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -66,7 +66,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -83,7 +83,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -100,7 +100,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "libName": "BackendDriver",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -116,7 +116,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "libName": "REVLibWpi",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,