Merge master

.pathplanner/settings.json

@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.88,
+  "robotLength": 0.88,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.03
+}

.roboRIOTeamNumberSetter/roborioteamnumbersetter.json

@@ -0,0 +1 @@
+{}

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,17 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "Shuffleboard": {
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/deploy/example.txt

@@ -1,3 +0,0 @@
-Files placed in this directory will be deployed to the RoboRIO into the
-'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
-to get a proper path relative to the deploy directory.

src/main/deploy/pathplanner/autos/From Subwoofer Far Edge to Amp.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7046435911053639,
+      "y": 4.3751818412727745
+    },
+    "rotation": 120.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "From Subwoofer Far Edge to Amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/From Subwoofer Front to Amp.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.29,
+      "y": 5.58
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "From Subwoofer Front to Amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7046435911053639,
+        "y": 4.3751818412727745
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.4368920788482864,
+        "y": 3.1622483921028106
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.82409310273166,
+        "y": 7.565612949407717
+      },
+      "prevControl": {
+        "x": 1.8176568333482528,
+        "y": 6.336387277909188
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 89.21368742867472,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 122.73522627210761,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/From Subwoofer Front to Amp.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2945574370576298,
+        "y": 5.580559282764361
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2786077412442953,
+        "y": 5.580559282764361
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.8232420006879377,
+        "y": 7.533755031731888
+      },
+      "prevControl": {
+        "x": 1.8232420006879377,
+        "y": 6.694612522270728
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -4,17 +4,9 @@
 
 package frc.robot;
 
-import java.io.IOException;
-import java.util.Optional;
-
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -22,217 +14,72 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.OIConstants;
-import frc.robot.constants.PhotonConstants;
-import frc.robot.constants.ShooterConstants;
-import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
-import frc.robot.subsystems.Indexer;
-import frc.robot.utilities.PhotonVision;
-import frc.robot.subsystems.Intake;
-import frc.robot.subsystems.Shooter;
+import frc.robot.utilities.PoseSendable;
 
 public class RobotContainer {
-    private static final String kAutoTabName = "Autonomous";
-    private static final String kTeleopTabName = "Teloperated";
-
-    //private PhotonVision photonvision;
-
     private Drivetrain drivetrain;
-    private Intake intake;
-    private Shooter shooter;
-    private Climber climber;
-    private Indexer indexer;
 
     private CommandXboxController driver;
-    private CommandXboxController operator;
 
-    private Trigger isTeleopTrigger;
+    private PoseSendable poseSendable;
 
     private SendableChooser<Command> autoChooser;
 
-    private int ampTagID;
-
-    // TODO There's more than one source tag, how do we want to handle this?
-    private int sourceTagID;
-
-    // TODO There's more than one speaker tag, how do we want to handle this?
-    private int speakerTag;
-
     public RobotContainer() {
-        /*try {
-         
-        photonvision = new PhotonVision(
-            PhotonConstants.kCameraName, 
-            PhotonConstants.kCameraTransform, 
-            PhotonConstants.kCameraHeightMeters,
-            PhotonConstants.kCameraPitchRadians
-        );
-        } catch (IOException e) {
-        photonvision = null;
-        }*/
-
-        // TODO Need to provide a real initial pose
-        // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all
-        // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
-        drivetrain = new Drivetrain(new Pose2d(), null, null);
-
-        intake = new Intake();
-
-        shooter = new Shooter();
-
-        climber = new Climber(shooter.getShooterAngle());
-
-        indexer = new Indexer();
-
-        // An example Named Command, doesn't need to remain once we start actually adding real things
-        // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
-        NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
-
-        // TODO Specify a default auto string once we have one
-        autoChooser = AutoBuilder.buildAutoChooser();
+        drivetrain = new Drivetrain(new Pose2d());
 
         driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
-        operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
 
-        isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
+        poseSendable = new PoseSendable();
 
-        Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
-
-        if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
-            ampTagID = 5;
-            sourceTagID = 9;
-            speakerTag = 4; 
-        } else {
-            ampTagID = 6;
-            sourceTagID = 1;
-            speakerTag = 8;
-        }
+        autoChooser = AutoBuilder.buildAutoChooser();
 
         configureBindings();
-        shuffleboardSetup();
+        shuffleboardConfig();
     }
 
-    // The objective should be to have the subsystems expose methods that return commands
-    // that can be bound to the triggers provided by the CommandXboxController class.
-    // This mindset should help keep RobotContainer a little cleaner this year. 
-    // This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example),
-    // but generally reduce/avoid any situation where the keyword "new" is involved if you're working 
-    // with a subsystem
     private void configureBindings() {
-        isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
-
         drivetrain.setDefaultCommand(
             drivetrain.teleopCommand(
                 driver::getLeftY, 
                 driver::getLeftX, 
-                driver::getRightX, 
+                () -> { return -driver.getRightX(); }, 
                 OIConstants.kTeleopDriveDeadband
             )
         );
 
-        //intake.setDefaultCommand(intake.intakeUpCommand());
-        intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, driver::getLeftTriggerAxis));
+        driver.a().onTrue(drivetrain.zeroHeadingCommand());
 
-       // shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0));
-        shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
-
-        climber.setDefaultCommand(climber.stop());
-
-        indexer.setDefaultCommand(indexer.shootNote(operator::getRightTriggerAxis));
-
-        driver.povCenter().onFalse(
-            drivetrain.driveCardinal(
-                driver::getLeftY, 
-                driver::getLeftX, 
-                driver.getHID()::getPOV,
-                OIConstants.kTeleopDriveDeadband).until(
-                () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
-                )
-        );
-
-        // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
-        driver.a().onTrue(
-            drivetrain.driveAprilTagLock(
-                driver::getLeftY, 
-                driver::getLeftX, 
-                OIConstants.kTeleopDriveDeadband, 
-                ampTagID
-            ).until(
-                () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
-            )
-        );
-
-        // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
         driver.b().onTrue(
-            drivetrain.driveAprilTagLock(
-                driver::getLeftY, 
-                driver::getLeftX,  
-                OIConstants.kTeleopDriveDeadband, 
-                sourceTagID
-            ).until(
-                () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
-            )
-        );
-
-        // Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back
-        driver.x().onTrue(
-            drivetrain.driveAprilTagLock(
-                driver::getLeftY, 
-                driver::getLeftX, 
-                OIConstants.kTeleopDriveDeadband, 
-                speakerTag
-            ).until(
-                () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
-            )
-        );
-
-        // This was originally a run while held, not sure that's really necessary, change it if need be
-        driver.y().onTrue(drivetrain.zeroHeadingCommand());
-
-        // This was originally a run while held, not sure that's really necessary, change it if need be
-        driver.rightBumper().onTrue(drivetrain.setXCommand());
-
-        /*
-        * This has been added because interest has been expressed in trying field relative vs
-        * robot relative control. This should <i>default</i> to field relative, but give the option
-        * for the person practicing driving to push start on the driver's controller to quickly switch to
-        * the other style. 
-        * 
-        * If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
-        * mechanism will need to be devised, this will work for now. 
-        */
-        driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
-
-        driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
-
-        operator.y().onTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis));
-        
+            new InstantCommand(() -> {
+                System.out.println("X: " + poseSendable.getX());
+                System.out.println("Y: " + poseSendable.getY());
+                System.out.println("Rotation: " + poseSendable.getRotation());
+                drivetrain.resetOdometry(poseSendable.getPose());
+            }
+        ));
     }
 
-  private void shuffleboardSetup() {
-    ShuffleboardTab autoTab = Shuffleboard.getTab(kAutoTabName);
-    autoTab.add("Autonomous Selector", autoChooser)
+    private void shuffleboardConfig() {
+        ShuffleboardTab tab = Shuffleboard.getTab("Testing");
+        tab.add("Drivetrain Pose", poseSendable)
             .withPosition(0, 0)
-      .withSize(2, 1)
-      .withWidget(BuiltInWidgets.kComboBoxChooser);
+            .withSize(2, 3);
 
-    //Always select the auto tab on startup
-    Shuffleboard.selectTab(kAutoTabName);
+        tab.add("Auto Selector", autoChooser)
+            .withPosition(2, 0)
+            .withSize(2, 1);
 
-    ShuffleboardTab teleopTab = Shuffleboard.getTab(kTeleopTabName);
-    teleopTab.addBoolean("Field Relative Controls Enabled?", drivetrain::isFieldRelativeControl)
-        .withPosition(0, 0)
+        tab.addDouble("Current Pose Rotation", () -> { return drivetrain.getPose().getRotation().getDegrees(); })
+            .withPosition(2, 1)
             .withSize(2, 1)
-        .withWidget(BuiltInWidgets.kBooleanBox);
-    teleopTab.addBoolean("indexer beam break", indexer.getBeamBreak());
-    teleopTab.addBoolean("shooter beam break", shooter.getBeamBreak());
-    teleopTab.addDouble("shooter angle", shooter.getShooterAngle());
-    teleopTab.addDouble("intake angle", intake.getIntakeAngle());
+            .withWidget(BuiltInWidgets.kTextView);
     }
 
     public Command getAutonomousCommand() {
-        return autoChooser.getSelected();
+        return drivetrain.zeroHeadingCommand().andThen(autoChooser.getSelected());
+        
     }
 }

src/main/java/frc/robot/constants/AutoConstants.java

@@ -24,7 +24,7 @@ public final class AutoConstants {
         new PIDConstants(kPXController), 
         new PIDConstants(kPThetaController), 
         kMaxSpeedMetersPerSecond, 
-        Units.inchesToMeters(34.65), 
+        Units.inchesToMeters(Math.sqrt(Math.pow(14-1.75, 2) + Math.pow(14-1.75, 2))), 
         new ReplanningConfig()
     );
 

src/main/java/frc/robot/constants/ClimberConstants.java

@@ -1,6 +0,0 @@
-package frc.robot.constants;
-
-public class ClimberConstants {
-    public static final int kClimberMotor1CANID = 14;
-    public static final int kClimberMotor2CANID = 15;
-}

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -7,12 +7,12 @@ import edu.wpi.first.math.util.Units;
 public final class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 4.6;
+    public static final double kMaxSpeedMetersPerSecond = 4.1;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
-    public static final double kDirectionSlewRate = 2.4; // radians per second
-    public static final double kMagnitudeSlewRate = 3.2; // percent per second (1 = 100%)
-    public static final double kRotationalSlewRate = 4.0; // percent per second (1 = 100%)
+    public static final double kDirectionSlewRate = 4.8; // radians per second
+    public static final double kMagnitudeSlewRate = 6.4; // percent per second (1 = 100%)
+    public static final double kRotationalSlewRate = 8.0; // percent per second (1 = 100%)
 
     // Chassis configuration
     public static final double kTrackWidth = Units.inchesToMeters(28-1.75*2);
@@ -26,10 +26,10 @@ public final class DrivetrainConstants {
         new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
     // Angular offsets of the modules relative to the chassis in radians
-    public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
-    public static final double kFrontRightChassisAngularOffset = 0;
-    public static final double kBackLeftChassisAngularOffset = Math.PI;
-    public static final double kBackRightChassisAngularOffset = Math.PI / 2;
+    public static final double kFrontLeftChassisAngularOffset = Math.PI;
+    public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
+    public static final double kBackRightChassisAngularOffset = 0;
 
     // SPARK MAX CAN IDs
     public static final int kFrontLeftDrivingCanId = 8;
@@ -47,5 +47,5 @@ public final class DrivetrainConstants {
 
     public static final boolean kGyroReversed = true;
 
-    public static final double kRobotStartOffset = 90;
+    public static final double kRobotStartOffset = 0;
 }

src/main/java/frc/robot/constants/IndexerConstants.java

@@ -1,7 +0,0 @@
-package frc.robot.constants;
-
-public class IndexerConstants {
-    public static final int kIndexerID = 13;
-
-    public static final int kIndexerBeamBreakChannel = 2;
-}

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -1,22 +0,0 @@
-package frc.robot.constants;
-
-public class IntakeConstants {
-    public static final int kIntakePivotID = 10;
-    public static final int kIntakeRollerID = 12;
-
-    public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0));
-
-    public static final int kPivotCurrentLimit = 40;
-
-    public static final double kPIntake = 0;
-    public static final double kIIntake = 0;
-    public static final double kDIntake = 0;
-
-    public static final double kSFeedForward = 0;
-    public static final double kGFeedForward = 0;
-    public static final double kVFeedForward = 0;
-
-    public static final double kStartingAngle = Math.toRadians(105.0);
-    public static final double kUpAngle = Math.toRadians(90.0);
-    public static final double kDownAngle = Math.toRadians(-34.0);
-}

src/main/java/frc/robot/constants/PhotonConstants.java

@@ -1,17 +0,0 @@
-package frc.robot.constants;
-
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
-
-public class PhotonConstants {
-    public static final String kCameraName = "Camera_Module_v1";
-
-    // TODO Set this to something real if visual pose estimation is used
-    public static final Transform3d kCameraTransform = new Transform3d();
-
-    // TODO The camera will be moved, this is an old value that needs to update
-    public static final double kCameraHeightMeters = .517525;
-
-    // TODO The camera will be moved, this is an old value that needs to update
-    public static final double kCameraPitchRadians = Units.degreesToRadians(15);
-}

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -1,35 +0,0 @@
-package frc.robot.constants;
-
-public class ShooterConstants {
-    
-    public static final int kTopShooterID = 9;
-    public static final int kBottomShooterID = 11;
-    public static final int kShooterPivotID = 16;
-
-    public static final double kShooterP = 0.0;
-    public static final double kShooterI = 0.0;
-    public static final double kShooterD = 0.0;
-
-    public static final double kSShooterFF = 0.0;
-    public static final double kVShooterFF = 0.0;
-
-    public static final double kShooterPivotP = 0.0;
-    public static final double kShooterPivotI = 0.0;
-    public static final double kShooterPivotD = 0.0;
-
-    public static final double kShooterLoadAngle = Math.toRadians(-20.0);
-    public static final double kShooterAmpAngle = Math.toRadians(105.0);
-
-    public static final double kPivotConversion = 1/(40.0*(28.0/15.0));
-
-    public static final double kSShooterPivotFF = 0.0;
-    public static final double kGShooterPivotFF = 0.0;
-    public static final double kVShooterPivotFF = 0.0;
-
-    public static final double kMaxPivotSpeed = 0.0;
-    public static final double kMaxPivotAcceleration = 0.0;
-
-    public static final int kShooterEncoderChannelA = 0;
-    public static final int kShooterEncoderChannelB = 1;
-    public static final int kShooterBeamBreakChannel = 3;
-}

src/main/java/frc/robot/constants/SwerveModuleConstants.java

@@ -49,6 +49,6 @@ public class SwerveModuleConstants {
     public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake;
     public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake;
 
-    public static final int kDrivingMotorCurrentLimit = 60; // amps
+    public static final int kDrivingMotorCurrentLimit = 65; // amps
     public static final int kTurningMotorCurrentLimit = 30; // amps
 }

src/main/java/frc/robot/interfaces/IAprilTagProvider.java

@@ -1,34 +0,0 @@
-package frc.robot.interfaces;
-
-import java.util.OptionalDouble;
-
-/**
- * An interface which ensures a class can provide common AprilTag oriented
- * information from various sources in a consistent way.
- */
-public interface IAprilTagProvider {
-    /**
-     * A method to get the distance from <i>the camera</i> to the AprilTag specified
-     * 
-     * @param id The ID of the AprilTag to give a distance to
-     * @param targetHeightMeters The height of the AprilTag off the ground, in meters
-     * @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
-
-    /**
-     * A method to get the pitch from the center of the image of a particular AprilTag
-     * 
-     * @param id The ID of the AprilTag to get the pitch of
-     * @return The pitch, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagPitchByID(int id);
-
-    /**
-     * A method to get the yaw from the center of the image of a particular AprilTag
-     * 
-     * @param id The ID of the AprilTag to get the yaw of
-     * @return The yaw, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagYawByID(int id);
-}

src/main/java/frc/robot/interfaces/IVisualPoseProvider.java

@@ -1,27 +0,0 @@
-package frc.robot.interfaces;
-
-import java.util.Optional;
-
-import edu.wpi.first.math.geometry.Pose2d;
-
-/**
- * An interface which ensures a class' ability to provide visual pose information
- * in a consistent way
- */
-public interface IVisualPoseProvider {
-    /**
-     * A record that can contain the two elements necessary for a WPILIB
-     * pose estimator to use the information from a vision system as part of a full
-     * robot pose estimation
-     */
-    public record VisualPose(Pose2d visualPose, double timestamp) {}
-
-    /**
-     * Return a VisualPose or null if an empty Optional if none is available.
-     * Implementation should provide an empty response if it's unable to provide
-     * a reliable pose, or any pose at all.
-     * 
-     * @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided
-     */
-    public Optional<VisualPose> getVisualPose();
-}

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

@@ -1,46 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.function.DoubleSupplier;
-
-import com.ctre.phoenix.motorcontrol.VictorSPXControlMode;
-import com.ctre.phoenix.motorcontrol.can.VictorSPX;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.ClimberConstants;
-
-public class Climber extends SubsystemBase {
-    private VictorSPX motor1;
-    private VictorSPX motor2;
-
-    private DoubleSupplier shooterAngle;
-
-    //TODO What tells the climber to stop climbing when it achieves the target height?
-    public Climber(DoubleSupplier shooterAngle) {
-        if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
-            motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
-            motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
-
-            motor2.follow(motor1);
-        }
-    }
-
-    public Command setSpeedWithSupplier(DoubleSupplier speed) {
-        return run(() -> {
-
-            if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
-                motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
-            }
-        });
-    }
-
-    public Command setSpeed(double speed) {
-        return run(() -> {
-            motor1.set(VictorSPXControlMode.PercentOutput, speed);
-        });
-    }
-
-    public Command stop() {
-        return setSpeed(0);
-    }
-}

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

@@ -1,7 +1,6 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -13,8 +12,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
 
 import java.util.Optional;
-import java.util.OptionalDouble;
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.kauailabs.navx.frc.AHRS;
@@ -26,12 +23,7 @@ import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.utilities.MAXSwerveModule;
 import frc.robot.utilities.SwerveUtils;
-import frc.robot.interfaces.IAprilTagProvider;
-import frc.robot.interfaces.IVisualPoseProvider;
-import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.PIDCommand;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Drivetrain extends SubsystemBase {
@@ -47,15 +39,9 @@ public class Drivetrain extends SubsystemBase {
     private final SlewRateLimiter m_magLimiter;
     private final SlewRateLimiter m_rotLimiter;
 
-    private final IAprilTagProvider m_aprilTagProvider;
-
-    private final IVisualPoseProvider m_visualPoseProvider;
-
     // Odometry class for tracking robot pose
     private final SwerveDrivePoseEstimator m_poseEstimator;
 
-    private boolean fieldRelativeControl;
-
     // Slew rate filter variables for controlling lateral acceleration
     private double m_currentRotation;
     private double m_currentTranslationDir;
@@ -64,7 +50,7 @@ public class Drivetrain extends SubsystemBase {
     private double m_prevTime;
 
     /** Creates a new DriveSubsystem. */
-    public Drivetrain(Pose2d initialPose, IAprilTagProvider aprilTagProvider, IVisualPoseProvider visualPoseProvider) {
+    public Drivetrain(Pose2d initialPose) {
         m_frontLeft = new MAXSwerveModule(
             DrivetrainConstants.kFrontLeftDrivingCanId,
             DrivetrainConstants.kFrontLeftTurningCanId,
@@ -106,12 +92,6 @@ public class Drivetrain extends SubsystemBase {
             initialPose
         );
 
-        m_aprilTagProvider = aprilTagProvider;
-
-        m_visualPoseProvider = visualPoseProvider;
-
-        fieldRelativeControl = true;
-
         m_currentRotation = 0.0;
         m_currentTranslationDir = 0.0;
         m_currentTranslationMag = 0.0;
@@ -132,6 +112,7 @@ public class Drivetrain extends SubsystemBase {
             }, 
             this
         );
+
     }
 
     @Override
@@ -146,24 +127,6 @@ public class Drivetrain extends SubsystemBase {
                 m_rearRight.getPosition()
             }
         );
-
-        if (m_visualPoseProvider != null) {
-            Optional<VisualPose> vPose = m_visualPoseProvider.getVisualPose();
-
-            if (vPose.isPresent()) {
-                m_poseEstimator.addVisionMeasurement(vPose.get().visualPose(), vPose.get().timestamp());
-            }
-        }
-    }
-
-    public boolean isFieldRelativeControl() {
-        return fieldRelativeControl;
-    }
-
-    public Command toggleFieldRelativeControl() {
-        return runOnce(() -> {
-            fieldRelativeControl = !fieldRelativeControl;
-        });
     }
 
     /**
@@ -175,14 +138,6 @@ public class Drivetrain extends SubsystemBase {
         return m_poseEstimator.getEstimatedPosition();
     }
 
-    public double getPoseX(){
-        return m_poseEstimator.getEstimatedPosition().getX();
-    }
-
-    public double getPoseY(){
-        return m_poseEstimator.getEstimatedPosition().getY();
-    }
-
     /**
      * Resets the odometry to the specified pose.
      *
@@ -220,7 +175,7 @@ public class Drivetrain extends SubsystemBase {
      *                      field.
      * @param rateLimit     Whether to enable rate limiting for smoother control.
      */
-    public void drive(double xSpeed, double ySpeed, double rot, BooleanSupplier fieldRelative, boolean rateLimit) {
+    public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) {
         double xSpeedCommanded;
         double ySpeedCommanded;
 
@@ -274,8 +229,8 @@ public class Drivetrain extends SubsystemBase {
         double rotDelivered = m_currentRotation * DrivetrainConstants.kMaxAngularSpeed;
 
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
-            fieldRelative.getAsBoolean()
-                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle()))
+            fieldRelative
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_poseEstimator.getEstimatedPosition().getRotation())
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -300,88 +255,12 @@ public class Drivetrain extends SubsystemBase {
                 -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
                 -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
                 -MathUtil.applyDeadband(rotation.getAsDouble(), deadband), 
-                () -> fieldRelativeControl, 
-                false
+                true, 
+                true
             );
         });
     }
 
-    public Command driveCardinal(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier pov, double deadband) {
-        PIDController controller = new PIDController(
-            AutoConstants.kPHeadingController, 
-            0, 
-            AutoConstants.kDHeadingController
-        );
-        controller.enableContinuousInput(-180, 180);
-        
-        return new PIDCommand(
-            controller, 
-            this::getHeading, 
-            pov::getAsDouble, 
-            (output) -> {
-                this.drive(
-                    -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
-                    -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
-                    output, 
-                    () -> fieldRelativeControl, 
-                    true
-                );
-            }, 
-            this);
-    }
-
-    public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
-        if (m_aprilTagProvider == null) {
-            return new PrintCommand("No AprilTag Provider Available");
-        }
-
-        // TODO The process variable is different here than what these constants are used for, may need to use something different
-        PIDController controller = new PIDController(
-            AutoConstants.kPHeadingController, 
-            0, 
-            AutoConstants.kDHeadingController
-        );
-
-        return new PIDCommand(
-            controller, 
-            () -> {
-                OptionalDouble tagYaw = m_aprilTagProvider.getTagYawByID(tagID);
-
-                if (tagYaw.isEmpty()) {
-                    return 0;
-                }
-
-                return tagYaw.getAsDouble();
-            }, 
-            () -> { return 0; }, 
-            (output) -> {
-                // TODO Field relative or no? What makes sense here, since the intent is to orient to the tag, not the field, it shouldn't be affected by the drivers mode choice I don't think. 
-                this.drive(
-                    -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
-                    -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
-                    output, 
-                    () -> fieldRelativeControl, 
-                    true
-                );
-            }, 
-            this
-        );
-    }
-
-    /**
-     * Sets the wheels into an X formation to prevent movement.
-     */
-    public void setX() {
-        m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
-        m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
-        m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
-        m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
-    }
-
-    public Command setXCommand() {
-        return runOnce(this::setX);
-    }
-
     /**
      * Sets the swerve ModuleStates.
      *
@@ -413,10 +292,6 @@ public class Drivetrain extends SubsystemBase {
         return runOnce(this::zeroHeading);
     }
 
-    public void offsetZero(double angle){
-        m_gyro.setAngleAdjustment(angle);
-    }
-
     /**
      * Returns the heading of the robot.
      *
@@ -427,7 +302,7 @@ public class Drivetrain extends SubsystemBase {
     }
 
     public double getGyroAngle(){
-        return m_gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0) + DrivetrainConstants.kRobotStartOffset;
+        return m_gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 
     /**

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

@@ -1,51 +0,0 @@
-package frc.robot.subsystems;
-
-import com.revrobotics.CANSparkMax;
-
-import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
-
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IndexerConstants;
-
-public class Indexer extends SubsystemBase{
-    
-    private CANSparkMax indexerMotor;
-    private DigitalInput indexerBeamBreak;
-
-    public Indexer(){
-        indexerMotor = new CANSparkMax(IndexerConstants.kIndexerID, MotorType.kBrushed);
-
-        indexerMotor.setSmartCurrentLimit(40);
-        indexerMotor.setIdleMode(IdleMode.kBrake);
-        indexerMotor.burnFlash();
-
-        indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakChannel);
-    }
-
-    public Command autoIndexing(){
-     return run(() -> {
-        if(!indexerBeamBreak.get()){
-            indexerMotor.set(0.5);
-        }else if(indexerBeamBreak.get()){
-            indexerMotor.set(0.0);
-        }
-
-     });
-    }
-    
-    public Command shootNote(DoubleSupplier indexerSpeed){
-        return run(() -> {
-            indexerMotor.set(indexerSpeed.getAsDouble());
-        });
-    }
-
-    public BooleanSupplier getBeamBreak(){
-        return indexerBeamBreak::get;
-    }
-}

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

@@ -1,115 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.function.DoubleSupplier;
-
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IntakeConstants;
-
-public class Intake extends SubsystemBase{
-    private PIDController intakeAnglePID;
-
-    private CANSparkMax intakeRoller;
-    private CANSparkMax intakePivot;
-
-    private RelativeEncoder intakeEncoder;
-
-    private ArmFeedforward intakeFeedForward;
-
-    public Intake(){
-        intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
-        
-        intakeRoller.setSmartCurrentLimit(60);
-        intakeRoller.setIdleMode(IdleMode.kCoast);
-        intakeRoller.burnFlash();
-
-        intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless);
-
-        intakePivot.setIdleMode(IdleMode.kBrake);
-        intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit);
-        intakePivot.burnFlash();
-
-        intakeFeedForward = new ArmFeedforward(
-            IntakeConstants.kSFeedForward, 
-            IntakeConstants.kGFeedForward, 
-            IntakeConstants.kVFeedForward
-        );
-
-        intakeEncoder = intakePivot.getEncoder();
-        intakeEncoder.setPosition(IntakeConstants.kStartingAngle);
-        intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
-
-        intakeAnglePID = new PIDController(
-            IntakeConstants.kPIntake, 
-            IntakeConstants.kIIntake, 
-            IntakeConstants.kDIntake
-        );
-
-    }
-
-    public Command intakeDownCommand() {
-        return run(() -> {
-            intakeRoller.set(1.0);
-        
-            intakePivot.setVoltage(
-                intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
-                    IntakeConstants.kDownAngle
-                ) + intakeFeedForward.calculate(
-                    IntakeConstants.kDownAngle, 
-                    intakeEncoder.getVelocity()
-                )
-            );
-        });
-    }
-
-    public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){
-        return run(() ->{
-            intakePivot.set(pivotPower.getAsDouble());
-            intakeRoller.set(rollerSpinny.getAsDouble());
-        });
-    }
-
-    public Command climbingStateCommand() {
-        return run(() -> {
-            intakeRoller.set(0.0);
-
-            intakePivot.setVoltage(
-                intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
-                    IntakeConstants.kDownAngle
-                ) + intakeFeedForward.calculate(
-                    IntakeConstants.kDownAngle, 
-                    intakeEncoder.getVelocity()
-                )
-            );
-        });
-    }
-
-    public Command intakeUpCommand() {
-        return run(() -> {
-            intakeRoller.set(0.0);
-
-            intakePivot.setVoltage(
-                intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
-                    IntakeConstants.kUpAngle
-                ) + intakeFeedForward.calculate(
-                    IntakeConstants.kUpAngle, 
-                    intakeEncoder.getVelocity()
-                )
-            );
-        });
-    }
-
-    public DoubleSupplier getIntakeAngle(){
-        return intakeEncoder::getPosition;
-    }
-}

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

@@ -1,148 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
-
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.ShooterConstants;
-
-public class Shooter extends SubsystemBase{
-    private CANSparkMax topShooter;
-    private CANSparkMax bottomShooter;
-
-    private RelativeEncoder bottomEncoder;
-    private RelativeEncoder topEncoder;
-
-    private CANSparkMax shooterPivot;
-
-    private Encoder pivotEncoder;
-
-    private PIDController topShooterPID;
-    private SimpleMotorFeedforward topShooterFF;
-
-    private PIDController bottomShooterPID;
-    private SimpleMotorFeedforward bottomShooterFF;
-
-    private PIDController shooterPivotPID;
-    private ArmFeedforward shooterPivotFF;
-
-    private DigitalInput shooterBeamBreak;
-
-    public Shooter(){
-        topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
-        bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
-        
-        topShooter.setSmartCurrentLimit(40);
-        topShooter.burnFlash();
-
-        bottomShooter.setSmartCurrentLimit(40);
-        bottomShooter.burnFlash();
-
-        topEncoder = topShooter.getEncoder();
-        bottomEncoder = bottomShooter.getEncoder();
-
-        shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
-
-        shooterPivot.setSmartCurrentLimit(50);
-        shooterPivot.burnFlash();
-
-        pivotEncoder = new Encoder(0, 1);
-        pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
-
-        shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
-
-        //TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed
-        topShooterPID = new PIDController(
-            ShooterConstants.kShooterP, 
-            ShooterConstants.kShooterI, 
-            ShooterConstants.kShooterD
-        );
-        bottomShooterPID = new PIDController(
-            ShooterConstants.kShooterP, 
-            ShooterConstants.kShooterI, 
-            ShooterConstants.kShooterD
-        );
-
-        topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
-        topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
-
-        shooterPivotPID = new PIDController(
-            ShooterConstants.kShooterPivotP,
-            ShooterConstants.kShooterPivotI,
-            ShooterConstants.kShooterPivotD
-        );
-
-        shooterPivotFF = new ArmFeedforward(
-            ShooterConstants.kSShooterPivotFF,
-            ShooterConstants.kGShooterPivotFF, 
-            ShooterConstants.kVShooterPivotFF
-        );
-    }
-
-    public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
-        return run(()-> {
-            shooterPivot.setIdleMode(IdleMode.kBrake);
-
-            shooterPivot.setVoltage(
-                shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + 
-                shooterPivotFF.calculate(setpointAngle, 0.0));
-
-            topShooter.setVoltage(topShooterPID.calculate(topEncoder.getVelocity(), topRPM)+topShooterFF.calculate(topRPM));
-
-            bottomShooter.setVoltage(bottomShooterPID.calculate(bottomEncoder.getVelocity(), bottomRPM)+bottomShooterFF.calculate(bottomRPM));
-        });
-    }
-
-    public Command recieveNote(){
-        return run(() -> {
-            shooterPivot.setIdleMode(IdleMode.kBrake);
-
-            shooterPivot.setVoltage(
-                shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) + 
-                shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
-            
-            if(shooterBeamBreak.get()){
-                topShooter.set(0.25);
-                bottomShooter.set(0.25);
-            }else{
-                topShooter.set(0.);
-                bottomShooter.set(0.); 
-            }
-        });
-    }
-
-    public Command climbState(){
-        return run(() -> {
-            shooterPivot.setIdleMode(IdleMode.kCoast);
-            shooterPivot.set(0.0);
-        });
-    }
-
-    public DoubleSupplier getShooterAngle(){
-        return () -> {return pivotEncoder.getDistance();};
-    }
-
-    public BooleanSupplier getBeamBreak(){
-        return shooterBeamBreak::get;
-    }
-
-    public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){
-        return run(() ->{
-            shooterPivot.set(pivotPower.getAsDouble());
-            topShooter.set(spinny.getAsDouble());
-            bottomShooter.set(spinny.getAsDouble());
-        });
-    }
-
-}

src/main/java/frc/robot/utilities/PhotonVision.java

@@ -1,135 +0,0 @@
-package frc.robot.utilities;
-
-import java.io.IOException;
-import java.util.List;
-import java.util.Optional;
-import java.util.OptionalDouble;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonUtils;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
-import frc.robot.interfaces.IAprilTagProvider;
-import frc.robot.interfaces.IVisualPoseProvider;
-
-public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
-
-    private final PhotonCamera camera;
-
-    private final PhotonPoseEstimator photonPoseEstimator;
-
-    private final double cameraHeightMeters;
-    private final double cameraPitchRadians;
-
-    public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
-        camera = new PhotonCamera(cameraName);
-
-        photonPoseEstimator = new PhotonPoseEstimator(
-            AprilTagFieldLayout.loadFromResource(
-                AprilTagFields.k2024Crescendo.m_resourceFile
-            ), 
-            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
-            camera,
-            robotToCam
-        );
-
-        this.cameraHeightMeters = cameraHeightMeters;
-        this.cameraPitchRadians = cameraPitchRadians;
-    }
-
-    @Override
-    public Optional<VisualPose> getVisualPose() {
-        Optional<EstimatedRobotPose> pose = photonPoseEstimator.update();
-
-        if (pose.isEmpty()) {
-            return Optional.empty();
-        }
-
-        return Optional.of(
-            new VisualPose(
-                pose.get().estimatedPose.toPose2d(), 
-                pose.get().timestampSeconds
-            )
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
-        PhotonPipelineResult result = camera.getLatestResult();
-        
-        if (!result.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            PhotonUtils.calculateDistanceToTargetMeters(
-                cameraHeightMeters, 
-                targetHeightMeters, 
-                cameraPitchRadians, 
-                Units.degreesToRadians(desiredTarget.get().getPitch()))
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagPitchByID(int id) {
-        PhotonPipelineResult result = camera.getLatestResult();
-        
-        if (!result.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getPitch()
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagYawByID(int id) {
-        PhotonPipelineResult result = camera.getLatestResult();
-        
-        if (!result.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getYaw()
-        );
-    }
-
-    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
-        for (PhotonTrackedTarget target : targets) {
-            if (target.getFiducialId() == id) {
-                return Optional.of(target);
-            }
-        }
-
-        return Optional.empty();
-    }
-    
-}

src/main/java/frc/robot/utilities/PoseSendable.java

@@ -0,0 +1,62 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+public class PoseSendable implements Sendable {
+
+    private double x;
+    private double y;
+    private double rot;
+
+    public PoseSendable() {
+        this(0, 0, 0);
+    }
+
+    public PoseSendable(double x, double y, double rot) {
+        this.x = x;
+        this.y = y;
+        this.rot = rot;
+    }
+
+    public double getX() {
+        return x;
+    }
+
+    public double getY() {
+        return y;
+    }
+
+    public double getRotation() {
+        return rot;
+    }
+
+    public Rotation2d getRotation2d() {
+        return Rotation2d.fromDegrees(rot);
+    }
+
+    public Pose2d getPose() {
+        return new Pose2d(x, y, getRotation2d());
+    }
+
+    public void setX(double x) {
+        this.x = x;
+    }
+
+    public void setY(double y) {
+        this.y = y;
+    }
+
+    public void setRotation(double rot) {
+        this.rot = rot;
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        builder.addDoubleProperty("X", this::getX, this::setX);
+        builder.addDoubleProperty("Y", this::getY, this::setY);
+        builder.addDoubleProperty("Rotation", this::getRotation, this::setRotation);
+    }
+}

vendordeps/PathplannerLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.1.2",
+    "version": "2024.2.4",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.1.2"
+            "version": "2024.2.4"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.1.2",
+            "version": "2024.2.4",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

vendordeps/REVLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2024.2.0",
+    "version": "2024.2.2",
     "frcYear": "2024",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2024.2.0"
+            "version": "2024.2.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2024.2.0",
+            "version": "2024.2.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -37,7 +37,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2024.2.0",
+            "version": "2024.2.2",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -55,7 +55,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2024.2.0",
+            "version": "2024.2.2",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,