vision formatted

check_style.xml

@@ -268,7 +268,6 @@
     </module>
     <module name="NoWhitespaceBeforeCaseDefaultColon"/>
     <module name="OverloadMethodsDeclarationOrder"/>
-    <module name="VariableDeclarationUsageDistance"/>
     <module name="CustomImportOrder">
       <property name="sortImportsInGroupAlphabetically" value="true"/>
       <property name="separateLineBetweenGroups" value="true"/>

src/main/java/frc/robot/commands/sequences/AutoIntakeSequence.java

@@ -12,21 +12,32 @@
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.vision.NoteDetectionWrapper;
 
+/**
+ * Detects a note, moves the robot to it, then intakes it.
+ */
 public class AutoIntakeSequence extends SequentialCommandGroup {
 
-
+    /**
+     * Constructs a new {@link AutoIntakeSequence}.
+     *
+     * @param elevatorSubsystem The subsystem to lower the intake.
+     * @param intakeRollersSubsystem The subsystem to intake the note.
+     * @param swerveSubsystem The subsystem to move the robot to the note.
+     * @param noteDetector The note detector that will be used to identify and locate the note.
+     * @param ledSubsystem The subsystem to display that the intake is active.
+     */
     public AutoIntakeSequence(
         ElevatorSubsystem elevatorSubsystem, 
         IntakeRollersSubsystem intakeRollersSubsystem,
         SwerveSubsystem swerveSubsystem,
         NoteDetectionWrapper noteDetector,
         LEDSubsystem ledSubsystem
-    ){
+    ) {
         addCommands(new ElevatorToGroundCommand(elevatorSubsystem)
-        .andThen(new NoteAlignCommand(swerveSubsystem, noteDetector))
-        .andThen(new ParallelDeadlineGroup(
-            new IntakeRollerIntakeCommand(intakeRollersSubsystem, ledSubsystem).withTimeout(3),
-            new DriveForwardCommand(swerveSubsystem)))
+            .andThen(new NoteAlignCommand(swerveSubsystem, noteDetector))
+            .andThen(new ParallelDeadlineGroup(
+                new IntakeRollerIntakeCommand(intakeRollersSubsystem, ledSubsystem).withTimeout(3),
+                new DriveForwardCommand(swerveSubsystem)))
         );
     }
 }

src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java

@@ -2,18 +2,29 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.Command;
-
-import java.util.NoSuchElementException;
-
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.vision.NoteDetectionWrapper;
+import java.util.NoSuchElementException;
 
-public class NoteAlignCommand extends Command{
+/**
+ *  Rotates the robot to face directly towards a note on the ground.
+ */
+public class NoteAlignCommand extends Command {
     private final SwerveSubsystem swerveSubsystem;
     private final NoteDetectionWrapper noteDetector;
 
+    private static final double MAX_ROTATION_POWER = 0.3;
+    private static final double ERROR_MULTIPLIER = 0.08;
+    private static final double ANGULAR_TOLERANCE_DEGREES = 1;
+
     private double noteYawOffsetDegrees;
 
+    /**
+     * Constructs a new {@link NoteAlignCommand}.
+     *
+     * @param swerveSubsystem The swerve subsystem to rotate the robot with.
+     * @param noteDetector The note detector that will be used to identify and locate the note.
+     */
     public NoteAlignCommand(SwerveSubsystem swerveSubsystem, NoteDetectionWrapper noteDetector) {
         this.swerveSubsystem = swerveSubsystem;
         this.noteDetector = noteDetector;
@@ -28,7 +39,7 @@ public void initialize() {
         this.noteYawOffsetDegrees = 0;
         try {
             noteYawOffsetDegrees = noteDetector.getNote().get().getYaw();
-        } catch(NoSuchElementException e) {
+        } catch (NoSuchElementException e) {
             System.out.println("Tried to align to a note, but none was detected.");
             this.end(true);
         }
@@ -39,18 +50,22 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return Math.abs(noteYawOffsetDegrees) < 1;
+        return Math.abs(noteYawOffsetDegrees) < ANGULAR_TOLERANCE_DEGREES;
     }
 
     @Override
     public void execute() {
         try {
             noteYawOffsetDegrees = noteDetector.getNote().get().getYaw();
-        } catch(NoSuchElementException e) {
-
+        } catch (NoSuchElementException e) {
+            /* It's usually fine if the note can't be detected for a few cycles, as long as it is detected again soon
+            after. If it remains out of frame, this issue should be rather evident (the robot will spin in place) and
+            is probably not worth logging. */
         }
 
-        swerveSubsystem.setRobotRelativeDrivePowers(0.,0., -MathUtil.clamp(noteYawOffsetDegrees * .008, -.3, .3));
+        swerveSubsystem.setRobotRelativeDrivePowers(0, 0, -MathUtil.clamp(
+            noteYawOffsetDegrees * ERROR_MULTIPLIER, -MAX_ROTATION_POWER, MAX_ROTATION_POWER)
+        );
     }
 
     @Override

src/main/java/frc/robot/vision/ApriltagWrapper.java

@@ -1,12 +1,6 @@
 package frc.robot.vision;
 
-import java.io.IOException;
-import java.util.Optional;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import static frc.robot.Constants.VisionConstants.VISION_TABLE_KEY;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -20,12 +14,16 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.wpilibj.Filesystem;
-
-import static frc.robot.Constants.VisionConstants.*;
+import java.io.IOException;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 
 /**
- * The ApriltagWrapper class gets robot pose estimates from a given camera and publishes robot pose estimations over
- * NetworkTables.
+ * Gets robot pose estimates from a given camera over PhotonVision and (optionally) publishes robot pose estimations
+ * over NetworkTables.
  */
 public class ApriltagWrapper {
     private final PhotonPoseEstimator poseEstimator;
@@ -38,6 +36,7 @@ public class ApriltagWrapper {
 
     /**
      * Constructs a PhotonVision connection to the coprocessor.
+     *
      * @param camera The PhotonVision camera object.
      * @param cameraPose The camera's 3d transformation relative to the robot.
      */
@@ -68,30 +67,33 @@ public ApriltagWrapper(PhotonCamera camera, Transform3d cameraPose) {
         DoubleTopic xPosTopic = visionTable.getDoubleTopic("xPos" + name);
         DoubleTopic yPosTopic = visionTable.getDoubleTopic("yPos" + name);
         DoubleTopic headingTopic = visionTable.getDoubleTopic("heading" + name);
-
+        
         this.debugEnabled = debugEnabledTopic.subscribe(false);
         this.xPosPub = xPosTopic.publish(PubSubOption.keepDuplicates(true));
         this.yPosPub = yPosTopic.publish(PubSubOption.keepDuplicates(true));
         this.headingPub = headingTopic.publish(PubSubOption.keepDuplicates(true));
     }
 
     /**
-     * Get the estimated robot pose from a single photon pose estimator.
+     * Gets the estimated robot pose from a single photon pose estimator.
+     *
      * @param prevEstimatedRobotPose The last aggregate robot pose estimate.
      * @return The optional vision-estimated pose.
      */
     public Optional<EstimatedRobotPose> getRobotPose(Pose3d prevEstimatedRobotPose) {
         poseEstimator.setReferencePose(prevEstimatedRobotPose);
         Optional<EstimatedRobotPose> robotPose = poseEstimator.update();
 
-        if (robotPose.isPresent() && debugEnabled.get())
+        if (robotPose.isPresent() && debugEnabled.get()) {
             updateNetworkTables(robotPose.get().estimatedPose.toPose2d());
+        }
 
         return robotPose;
     }
 
     /**
      * Updates the camera's NetworkTables topics with a pose estimate.
+     *
      * @param robotPose The robot pose to update NetworkTables with.
      */
     private void updateNetworkTables(Pose2d robotPose) {

src/main/java/frc/robot/vision/NoteDetectionWrapper.java

@@ -1,24 +1,31 @@
 package frc.robot.vision;
 
-import static frc.robot.Constants.VisionConstants.*;
-
-import java.util.Optional;
-
-import org.photonvision.PhotonCamera;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
+import static frc.robot.Constants.VisionConstants.VISION_TABLE_KEY;
 
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.BooleanTopic;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.PubSubOption;
+import java.util.Optional;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
+/**
+ * Detects notes' positions withing a given camera frame over PhotonVision and publishes note detections over
+ * NetworkTables.
+ */
 public class NoteDetectionWrapper {
     private final PhotonCamera camera;
 
     private final BooleanPublisher noteDetectedPub;
 
+    /**
+     * Constructs a {@link NoteDetectionWrapper}. 
+     *
+     * @param camera The PhotonVision camera object.
+     */
     public NoteDetectionWrapper(PhotonCamera camera) {
         this.camera = camera;
 
@@ -29,6 +36,12 @@ public NoteDetectionWrapper(PhotonCamera camera) {
         this.noteDetectedPub = noteDetectedTopic.publish(PubSubOption.keepDuplicates(true));
     }
 
+    /**
+     * Tries to detect a note. If successful, it will return information about its position within the frame and update
+     * the {@code noteDetected} boolean over NetworkTables. 
+     *
+     * @return Optionally, a note's {@link PhotonTrackedTarget} object.
+     */
     public Optional<PhotonTrackedTarget> getNote() {
         PhotonPipelineResult result = camera.getLatestResult();
         if (!result.hasTargets()) {