added some of the more important docstrings.. the rest will be added …

…later

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -106,13 +106,6 @@ public SwerveDrive(
     gyroDisconnectedAlert.set(false);
   }
 
-  /** Updates the pose estimator with the pose calculated from the swerve modules. */
-  // public void addPoseEstimatorSwerveMeasurement() {
-  //   for (int timestampIndex = 0;
-  //       timestampIndex < odometryThreadInputs.measurementTimeStamps.length;
-  //       timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex);
-  // }
-
   /*
    * Updates the pose estimator with the pose calculated from the april tags. How much it
    * contributes to the pose estimation is set by setPoseEstimatorVisionConfidence.
@@ -140,6 +133,7 @@ public void setPoseEstimatorVisionConfidence(
         VecBuilder.fill(xStandardDeviation, yStandardDeviation, thetaStandardDeviation));
   }
 
+  @Override
   public void periodic() {
     final double t0 = TimeUtil.getRealTimeSeconds();
     fetchOdometryInputs();
@@ -183,6 +177,7 @@ private void fetchOdometryInputs() {
     odometryThread.unlockOdometry();
   }
 
+  /** Runs the SwerveModules periodic methods */
   private void modulesPeriodic() {
     for (SwerveModule module : swerveModules) module.periodic();
   }
@@ -289,23 +284,6 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
    *
    * @param timestampIndex index of the timestamp to sample the pose at
    */
-  // private void addPoseEstimatorSwerveMeasurement(int timestampIndex) {
-  //   final SwerveModulePosition[] modulePositions = getModulePositions(),
-  //       moduleDeltas = getModulesDelta(modulePositions);
-
-  //   if (gyroInputs.isConnected) {
-  //     rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex];
-  //   } else {
-  //     Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas);
-  //     rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
-  //   }
-
-  //   poseEstimator.updateWithTime(
-  //       odometryThreadInputs.measurementTimeStamps[timestampIndex],
-  //       rawGyroRotation,
-  //       modulePositions);
-  // }
-
   public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex
     final SwerveModulePosition[] modulePositions = getModulePositions(),
         moduleDeltas = getModulesDelta(modulePositions);
@@ -324,21 +302,9 @@ public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex
   }
 
   /**
-   * Gets the modules positions, sampled at the indexed timestamp.
-   *
-   * @param timestampIndex the timestamp index to sample.
-   * @return a list of SwerveModulePosition, containing relative drive position and absolute turn
-   *     rotation at the sampled timestamp.
+   * @param freshModulesPosition Latest module positions
+   * @return The change of the module positions between the current and last update
    */
-  // private SwerveModulePosition[] getModulesPosition(int timestampIndex) {
-  //   SwerveModulePosition[] swerveModulePositions = new
-  // SwerveModulePosition[swerveModules.length];
-  //   for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++)
-  //     swerveModulePositions[moduleIndex] =
-  //         swerveModules[moduleIndex].getOdometryPositions()[timestampIndex];
-  //   return swerveModulePositions;
-  // }
-
   private SwerveModulePosition[] getModulesDelta(SwerveModulePosition[] freshModulesPosition) {
     SwerveModulePosition[] deltas = new SwerveModulePosition[swerveModules.length];
     for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {

src/main/java/frc/robot/subsystems/swerve/SwerveModule.java

@@ -32,27 +32,28 @@ public SwerveModule(ModuleInterface io, String name) {
     CommandScheduler.getInstance().unregisterSubsystem(this);
   }
 
+  /** Updates the module's odometry inputs. */
   public void updateOdometryInputs() {
     io.updateInputs(inputs);
     Logger.processInputs("Drive/Module-" + name, inputs);
     this.hardwareFaultAlert.set(!inputs.isConnected);
   }
 
   @Override
-  public void periodic() {
-    // updateOdometryPositions();
-    Logger.recordOutput("Drive/current turn angle", getTurnRotation().getRotations());
-  }
+  public void periodic() {}
 
+  /** Sets the drive voltage of the module. */
   public void setVoltage(Voltage volts) {
     io.setDriveVoltage(volts);
     io.setTurnVoltage(Volts.zero());
   }
 
+  /** Gets the drive voltage of the module. */
   public double getDriveVoltage() {
     return inputs.driveAppliedVolts;
   }
 
+  /** Sets the drive velocity of the module. */
   public double getCharacterizationVelocity() {
     return inputs.driveVelocity;
   }

src/main/java/frc/robot/subsystems/vision/PhysicalVision.java

@@ -297,6 +297,12 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) {
     this.headingRateDegreesPerSecond = headingRateDegrees;
   }
 
+  /**
+   * Checks if the specified limelight is connected
+   *
+   * @param limelight The limelight to check
+   * @return True if the limelight network table contains the key "tv"
+   */
   public boolean isLimelightConnected(Limelight limelight) {
     NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(limelight.getName());
     return limelightTable.containsKey("tv");
@@ -403,7 +409,6 @@ public void stopLimelightThread(Limelight limelight) {
   }
 
   /** Shuts down all the threads. */
-  // @Override
   public void endAllThreads() {
     threadManager.shutdownAllThreads();
   }

src/main/java/frc/robot/subsystems/vision/SimulatedVision.java

@@ -106,6 +106,13 @@ public void updateInputs(VisionInputs inputs) {
     }
   }
 
+  /**
+   * Writes photonvision results to the limelight network table
+   *
+   * @param results The simulated photonvision pose estimation results
+   * @param table The Limelight table to write to
+   * @param limelight The Limelight to write for
+   */
   private void writeToTable(
       List<PhotonPipelineResult> results, NetworkTable table, Limelight limelight) {
     // write to ll table
@@ -159,7 +166,13 @@ private void writeToTable(
     }
   }
 
-  public PhotonCamera getSimulationCamera(Limelight limelight) {
+  /**
+   * Gets the PhotonCamera for the given Limelight
+   *
+   * @param limelight The Limelight to get the camera for
+   * @return A PhotonCamera object for the given Limelight
+   */
+  private PhotonCamera getSimulationCamera(Limelight limelight) {
     return switch (limelight) {
       case SHOOTER -> VisionConstants.SHOOTER_CAMERA;
       case FRONT_LEFT -> VisionConstants.FRONT_LEFT_CAMERA;
@@ -168,7 +181,13 @@ public PhotonCamera getSimulationCamera(Limelight limelight) {
     };
   }
 
-  public NetworkTable getLimelightTable(Limelight limelight) {
+  /**
+   * Gets the Limelight network table
+   *
+   * @param limelight The Limelight to get the table for
+   * @return The network table of the Limelight
+   */
+  private NetworkTable getLimelightTable(Limelight limelight) {
     return switch (limelight) {
       case SHOOTER -> LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName());
       case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName());

src/main/java/frc/robot/subsystems/vision/VisionInterface.java

@@ -23,34 +23,72 @@ class VisionInputs {
     public double[] limelightTimestamps = new double[Limelight.values().length];
   }
 
+  /**
+   * Updates the inputs for the vision subsystem using VisionInputs
+   *
+   * @param inputs the inputs to update
+   */
   default void updateInputs(VisionInputs inputs) {}
 
+  /**
+   * @param limelight The Limelight to retrieve the latency data from
+   * @return The current latency of the Limelight
+   */
   default double getLatencySeconds(Limelight limelight) {
     return 0.0;
   }
 
+  /**
+   * @param limelight The Limelight to retrieve the timestamp data from
+   * @return The current timestamp of the Limelight
+   */
   default double getTimeStampSeconds(Limelight limelight) {
     return 0.0;
   }
 
+  /**
+   * @param limelight The Limelight to retrieve the target data from
+   * @return Whether the Limelight can see any targets, and if it is within its field of view
+   */
   default boolean canSeeAprilTags(Limelight limelight) {
     return false;
   }
 
+  /**
+   * @param limelight The Limelight to retrieve the distance data from
+   * @return The current April Tag distance from the Limelight
+   */
   default double getLimelightAprilTagDistance(Limelight limelight) {
     return 0.0;
   }
 
+  /**
+   * @param limelight The Limelight to retrieve the data from
+   * @return The current number of April Tags of the Limelight
+   */
   default int getNumberOfAprilTags(Limelight limelight) {
     return 0;
   }
 
+  /**
+   * @param limelight The Limelight to retrieve the pose data from
+   * @return The current pose of the Limelight
+   */
   default Pose2d getPoseFromAprilTags(Limelight limelight) {
     return null;
   }
 
+  /**
+   * Sets the heading information of the robot, used with MT2
+   *
+   * @param headingDegrees The heading of the robot in degrees
+   * @param headingRateDegrees The rate of change of the heading of the robot in degrees
+   */
   default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {}
 
+  /**
+   * @return The last seen pose of any Limelight that most recently saw a target
+   */
   default Pose2d getLastSeenPose() {
     return null;
   }