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;
+        
+        public static final int SHOOTER_MOTOR_CURRENT_LIMIT = 80;
 
-        private static GenericEntry indexerAndRampSpeed = programmingTab.add("Desired Ramp + Indexer Speed", 0.6)
+        public static final double SHOOTER_MOTOR_P = 0.0018;
-                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+        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();
     }
 
     /**
@@ -89,11 +78,10 @@ public class Robot extends TimedRobot {
         // block in order for anything in the Command-based framework to work.
         CommandScheduler.getInstance().run();
 
-       // Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
+        //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,8 +76,9 @@ 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()
-            driverXbox::getRightY)
+            .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,10 +227,13 @@ 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)
-                    () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
+                    .whileTrue(Commands.runEnd(
+                            () -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
+                            () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
 
             // driverXbox.b().whileTrue(
             // drivebase.driveToPose(
@@ -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

@@ -8,16 +8,16 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
-public class ClimberSubsystem extends SubsystemBase{
+public class ClimberSubsystem extends SubsystemBase {
     private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
     private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT);
 
     public void liftRobot() {
-        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED);
+        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE);
     }
 
     public void lowerRobot() {
-        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
+        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE * -1);
     }
 
     public void stopClimber() {
@@ -37,11 +37,11 @@ public class ClimberSubsystem extends SubsystemBase{
     }
 
     public static void toggleRatchet(boolean toggle) {
-    if (toggle == true) {
+      if (toggle == true) {
-      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
+        climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE_DEGREES);
-    } else
+      } else
-      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
+        climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE_DEGREES);
-  }
+    }
 
   public Command toggleRatchetCommand(boolean toggle) {
     return runOnce(() -> toggleRatchet(toggle));
@@ -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,94 +1,113 @@
-package frc.robot.subsystems;
+package frc.robot.subsystems;
-
+
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.math.controller.BangBangController;
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.Constants;
+import edu.wpi.first.wpilibj2.command.Command;
-
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import com.revrobotics.spark.ClosedLoopSlot;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
-import com.revrobotics.spark.SparkBase.PersistMode;
+import frc.robot.Constants;
-import com.revrobotics.spark.SparkBase.ResetMode;
+
-import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
-import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
-import com.revrobotics.spark.config.SparkFlexConfig;
+
-import com.revrobotics.RelativeEncoder;
+public class IntakeSubsystem extends SubsystemBase {
-import com.revrobotics.spark.SparkBase.ControlType;
+
-
+    private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID, MotorType.kBrushless);
-public class IntakeSubsystem extends SubsystemBase {
+
-
+    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, MotorType.kBrushless);
-    private static SparkFlex intakeMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
+
-            MotorType.kBrushless);
+    public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
-
+    private static BangBangController intakeWheelsMotorBBController = new BangBangController();
-    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
+    private static SimpleMotorFeedforward intakeWheelsMotorFeedforward;
-            MotorType.kBrushless);
+
-    private static SparkClosedLoopController intakeRotatorPIDController;
+    public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
-    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,
+    public IntakeSubsystem() {
-                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
+        intakeWheelsMotorConfig
-                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D);
+            .smartCurrentLimit(Constants.IntakeConstants.INTAKE_WHEELS_CURRENT_LIMIT)
-        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+            .encoder
-                com.revrobotics.PersistMode.kNoPersistParameters);
+            .positionConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_POSITION_CONVERSION_FACTOR)
-        intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
+            .velocityConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR);
-    }
+        intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
-
+
-    public void startIntakeMotor() {
+        intakeWheelsMotorFeedforward = new SimpleMotorFeedforward(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_S, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
-        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED);
+        
-    }
+        intakeRotatorConfig
-
+            .smartCurrentLimit(Constants.IntakeConstants.INTAKE_ROTATOR_CURRENT_LIMIT)
-    public void reverseIntakeMotor() {
+            .encoder
-        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED * -1);
+            .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);
-    public void stopIntakeMotor() {
+        intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_ROTATOR_INITIAL_ENCODER_VALUE);
-        intakeMotor.set(0);
+
-    }
+        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 Command startIntakeMotorCommand() {
+    }
-        return runOnce(() -> startIntakeMotor());
+
-    }
+    public void startIntakeMotor() {
-
+        intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM);
-    public Command reverseIntakeMotorCommand() {
+    }
-        return runOnce(() -> reverseIntakeMotor());
+
-    }
+    public void reverseIntakeMotor() {
-
+        intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1);
-    public Command stopIntakeMotorCommand() {
+    }
-        return runOnce(() -> stopIntakeMotor());
+
-    }
+    public void stopIntakeMotor() {
-
+        intakeWheelsMotorBBController.setSetpoint(0.0);
-    public void deployIntake() {
+    }
-        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
+
-                ControlType.kPosition);
+    public Command startIntakeMotorCommand() {
-    }
+        return runOnce(() -> startIntakeMotor());
-
+    }
-    public Command deployintakeCommand() {
+
-        return runOnce(() -> deployIntake());
+    public Command reverseIntakeMotorCommand() {
-    }
+        return runOnce(() -> reverseIntakeMotor());
-
+    }
-    public void retractIntake() {
+
-        intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition);
+    public Command stopIntakeMotorCommand() {
-    }
+        return runOnce(() -> stopIntakeMotor());
-
+    }
-    public Command retractIntakeCommand() {
+
-        return runOnce(() -> retractIntake());
+    public void deployIntake() {
-    }
+        intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_POSITION_RADS);
-
+    }
-    public void assistFuelIntake() {
+
-        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
+    public Command deployintakeCommand() {
-                ControlType.kPosition);
+        return runOnce(() -> deployIntake());
-    }
+    }
-
+
-    public Command assistFuelIntakeCommand() {
+    public void retractIntake() {
-        return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
+        intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_POSITION_RADS);
-                .andThen(new WaitCommand(2));
+    }
-    }
+
-
+    public Command retractIntakeCommand() {
-    @Override
+        return runOnce(() -> retractIntake());
-    public void periodic() {
+    }
-        SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
+
-    }
+    public void assistFuelIntake() {
-}
+        intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_POSITION_RADS);
+    }
+
+    public Command assistFuelIntakeCommand() {
+        return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
+                .andThen(new WaitCommand(2));
+    }
+
+    @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,124 +1,217 @@
-package frc.robot.subsystems;
+package frc.robot.subsystems;
-
+
-import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Constants;
-import edu.wpi.first.wpilibj2.command.Command;
+
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.spark.SparkFlex;
-import frc.robot.Constants;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
-
+import com.revrobotics.spark.config.SparkFlexConfig;
-import java.util.function.BooleanSupplier;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
-
+import edu.wpi.first.math.controller.BangBangController;
-import com.ctre.phoenix6.controls.Follower;
+import edu.wpi.first.math.controller.PIDController;
-import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import com.revrobotics.spark.SparkBase.ControlType;
+
-import com.revrobotics.spark.SparkBase.PersistMode;
+public class ShooterSubsystem extends SubsystemBase {
-import com.revrobotics.spark.SparkBase.ResetMode;
+    private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
-import com.revrobotics.spark.SparkBase;
+            MotorType.kBrushless);
-import com.revrobotics.spark.SparkFlex;
+
-import com.revrobotics.spark.SparkMax;
+    private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID,
-import com.revrobotics.spark.SparkLowLevel.MotorType;
+            MotorType.kBrushless);
-import com.revrobotics.spark.config.SparkFlexConfig;
+
-import com.revrobotics.spark.config.SparkBaseConfig;
+    private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
-import edu.wpi.first.wpilibj2.command.WaitCommand;
+            MotorType.kBrushless);
-import frc.robot.LimelightHelpers;
+
-import edu.wpi.first.wpilibj.AnalogPotentiometer;
+    private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
-
+            MotorType.kBrushless);
-public class ShooterSubsystem extends SubsystemBase {
+
-    private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
+    public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig();
-            MotorType.kBrushless);
+    private static BangBangController centerShooterMotorBBController = new BangBangController();
-
+    private static SimpleMotorFeedforward centerShooterMotorFeedforward;
-    private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID,
+
-            MotorType.kBrushless);
+    public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
-
+    private static BangBangController leftShooterMotorBBController = new BangBangController();
-    private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
+    private static SimpleMotorFeedforward leftShooterMotorFeedforward;
-            MotorType.kBrushless);
+
-
+    public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig();
-    private static SparkFlex indexerMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
+    private static BangBangController rightShooterMotorBBController = new BangBangController();
-            MotorType.kBrushless);
+    private static SimpleMotorFeedforward rightShooterMotorFeedforward;
-
+
-    //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
+    public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig();
-           // MotorType.kBrushless);
+    private static PIDController indexerAndRampMotorPIDController;
-
+
-    //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
+    public ShooterSubsystem() {
-            //MotorType.kBrushless);
+        centerShooterMotorConfig.smartCurrentLimit(60);
-
+        centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-    //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0);
+            com.revrobotics.PersistMode.kNoPersistParameters);
-    //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0);
+        centerShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.CENTER_MOTOR_S, Constants.ShooterConstants.CENTER_MOTOR_V);
-
+    
-    private static double currentPotentiometerPosition; // might need second value for the right potentiometer
+        leftShooterMotorConfig.smartCurrentLimit(60);
-
+        leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-    public void startShooterMotors() {
+            com.revrobotics.PersistMode.kNoPersistParameters);
-        centerShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        leftShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.LEFT_MOTOR_S, Constants.ShooterConstants.LEFT_MOTOR_V);
-        leftShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+    
-        rightShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        rightShooterMotorConfig.smartCurrentLimit(60);
-    }
+        rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-
+            com.revrobotics.PersistMode.kNoPersistParameters);
-    public double getShooterMotorVelocity() {
+        rightShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.RIGHT_MOTOR_S, Constants.ShooterConstants.RIGHT_MOTOR_V);
-        return leftShooterMotor.getEncoder().getVelocity();
+
-    }
+        indexerAndRampMotorConfig.smartCurrentLimit(60);
-
+        indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-    public void startIndexerMotor() {
+            com.revrobotics.PersistMode.kNoPersistParameters);
-        // if (LimelightHelpers.getTX("limelight") < 1.5 &&
+        indexerAndRampMotorPIDController = new PIDController(Constants.ShooterConstants.INDEXER_MOTOR_P, Constants.ShooterConstants.INDEXER_MOTOR_I, Constants.ShooterConstants.INDEXER_MOTOR_D);
-        // LimelightHelpers.getTX("limelight") > -1.5) {
+    }
-        indexerMotor.set(Constants.ShooterConstants.INDEXER_MOTOR_SPEED);
+
-        // } else
+
-        // indexerMotor.set(0);
+    //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
-    }
+      //      MotorType.kBrushless);
-
+
-    /* public Command shootFuelCommand() {
+    //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
-        return run(() -> startShooterMotors())
+            //MotorType.kBrushless);
-                .until(() -> {
+
-                    return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY);
+    //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0);
-                })
+    //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0);
-                .andThen(() -> startIndexerMotor());
+
-    } */
+    private static double currentPotentiometerPosition; // might need second value for the right potentiometer
-
+
-    
+    public void setShooterMotorsRPM() {
-      public Command shootFuelCommand() {
+        centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-      return runOnce(() -> startShooterMotors()).andThen(new WaitCommand(2))
+        leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-      .andThen(() -> startIndexerMotor());
+        rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-      };
+    }
-     
+
-
+    //test individual motor code
-    public void stopShooters() {
+    public void setLeftShooterMotorRPM() {
-        centerShooterMotor.set(0);
+        leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-        leftShooterMotor.set(0);
+    }
-        rightShooterMotor.set(0);
+    public Command testLeftShooterCommand() {
-        indexerMotor.set(0);
+        return runOnce(() -> setLeftShooterMotorRPM());
-    }
+    }
-
+
-    public Command stopShooterCommand() {
+    public void setRightShooterMotorRPM() {
-        return runOnce(() -> stopShooters());
+        rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-    }
+    }
-
+    public Command testRightShooterCommand() {
-    public void moveActuator(double desiredPotentiometerPosition) {
+        return runOnce(() -> setRightShooterMotorRPM());
-        if (desiredPotentiometerPosition > currentPotentiometerPosition) {
+    }
-            //TODO: Test for positive or negative power
+
-            //leftActuatorMotor.set(0.1);
+    public void setCenterShooterMotorRPM() {
-            //rightActuatorMotor.set(0.1);
+        centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
-        } else {
+    }
-            //leftActuatorMotor.set(-0.1);
+    public Command testCenterShooterCommand() {
-            //rightActuatorMotor.set(-0.1);
+        return runOnce(() -> setCenterShooterMotorRPM());
-        }
+    }
-    }
+
-
+    public void stopLeftShooterMotorRPM() {
-    public void stopActuator() {
+        leftShooterMotorBBController.setSetpoint(0.0);
-        //leftActuatorMotor.set(0);
+    }
-        //rightActuatorMotor.set(0);
+    public Command stopLeftShooterCommand() {
-    }
+        return runOnce(() -> stopLeftShooterMotorRPM());
-
+    }
-    public Command moveActuatorCommand(double desiredPotentiometerPosition) {
+
-        return run(() -> moveActuator(desiredPotentiometerPosition))
+    public void stopCenterShooterMotorRPM() {
-                .until(() -> currentPotentiometerPosition == currentPotentiometerPosition)
+        centerShooterMotorBBController.setSetpoint(0.0);
-                .andThen(() -> stopActuator());
+    }
-    }
+    public Command stopCenterShooterCommand() {
-
+        return runOnce(() -> stopCenterShooterMotorRPM());
-    @Override
+    }
-    public void periodic() {
+
-        /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
+    public void stopRightShooterMotorRPM() {
-        SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
+        rightShooterMotorBBController.setSetpoint(0.0);
-        currentPotentiometerPosition = leftPotentiometer.get(); */
+    }
-    }
+    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 runOnce(() -> setShooterMotorsRPM())
+                .until(() -> {return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM * 0.9);})
+                .andThen(() -> setIndexerAndRampMotorRPM()); 
+    } */
+
+    
+      public Command shootFuelCommand() {
+      return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
+      .andThen(() -> setIndexerAndRampMotorRPM());
+      }; 
+     
+
+    public void stopShooters() {
+        centerShooterMotorBBController.setSetpoint(0.0);
+        leftShooterMotorBBController.setSetpoint(0.0);
+        rightShooterMotorBBController.setSetpoint(0.0);
+        indexerAndRampMotorPIDController.setSetpoint(0.0);
+    }
+
+    public Command stopShooterCommand() {
+        return runOnce(() -> stopShooters());
+    }
+
+    public void moveActuator(double desiredPotentiometerPosition) {
+        if (desiredPotentiometerPosition > currentPotentiometerPosition) {
+            //TODO: Test for positive or negative power
+            //leftActuatorMotor.set(0.1);
+            //rightActuatorMotor.set(0.1);
+        } else {
+            //leftActuatorMotor.set(-0.1);
+            //rightActuatorMotor.set(-0.1);
+        }
+    }
+
+    public void stopActuator() {
+        //leftActuatorMotor.set(0);
+        //rightActuatorMotor.set(0);
+    }
+
+    public Command moveActuatorCommand(double desiredPotentiometerPosition) {
+        return run(() -> moveActuator(desiredPotentiometerPosition))
+                .until(() -> currentPotentiometerPosition == currentPotentiometerPosition)
+                .andThen(() -> stopActuator());
+    }
+
+    @Override
+    public void periodic() {
+        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/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,