From df753f21fcb595df86002cb14d1a5f8aaa354c44 Mon Sep 17 00:00:00 2001
From: jgelia <jgelia@ncsu.edu>
Date: Sun, 5 Jan 2025 13:09:44 -0500
Subject: [PATCH] Added comments and remove useless code

---
 .../frc/robot/commands/drive/DriveCommand.java     |  7 ++++---
 .../frc/robot/commands/drive/DriveCommandBase.java | 14 ++++----------
 2 files changed, 8 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/commands/drive/DriveCommand.java b/src/main/java/frc/robot/commands/drive/DriveCommand.java
index 83b1ac9..1a0d64d 100644
--- a/src/main/java/frc/robot/commands/drive/DriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive/DriveCommand.java
@@ -6,8 +6,6 @@
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
-// import frc.robot.subsystems.vision.VisionSubsystem;
-
 public class DriveCommand extends DriveCommandBase {
 
   private final SwerveDrive driveSubsystem;
@@ -38,6 +36,7 @@ public DriveCommand(
     super(driveSubsystem, visionSubsystem);
     this.driveSubsystem = driveSubsystem;
     addRequirements(driveSubsystem, visionSubsystem);
+
     this.leftJoystickY = leftJoystickY;
     this.leftJoystickX = leftJoystickX;
     this.rightJoystickX = rightJoystickX;
@@ -50,13 +49,15 @@ public void initialize() {}
 
   @Override
   public void execute() {
-    // Drives the robot
+    // Most of the time the driver prefers that the robot rotates slowly, as it gives them more control
+    // but sometimes (e.g. when fighting defense bots) being able to rotate quickly is necessary
     if (isHighRotation.getAsBoolean()) {
       angularSpeed = DriveConstants.MAX_ANGULAR_SPEED_RADIANS_PER_SECOND;
     } else {
       angularSpeed = DriveConstants.LOW_ANGULAR_SPEED_RADIANS_PER_SECOND;
     }
 
+    // Drives the robot by scaling the joystick inputs
     driveSubsystem.drive(
         leftJoystickY.getAsDouble() * DriveConstants.MAX_SPEED_METERS_PER_SECOND,
         leftJoystickX.getAsDouble() * DriveConstants.MAX_SPEED_METERS_PER_SECOND,
diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java
index b8136a3..7bed11d 100644
--- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java
+++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java
@@ -47,21 +47,16 @@ public void execute() {
   }
 
   public void calculatePoseFromLimelight(Limelight limelightNumber) {
-    double currentTimeStampSeconds = lastTimeStampSeconds;
-
     // Updates the robot's odometry with april tags
     if (vision.canSeeAprilTags(limelightNumber)) {
-      currentTimeStampSeconds = vision.getTimeStampSeconds(limelightNumber);
-
       double distanceFromClosestAprilTag = vision.getLimelightAprilTagDistance(limelightNumber);
 
       // Depending on how many april tags we see, we change our confidence as more april tags
       // results in a much more accurate pose estimate
-      // TODO: check if this is necessary anymore with MT2, also we might want to set the limelight
-      //  so it only uses 1 april tag, if they set up the field wrong (they can set april tags +-1
-      // inch I believe)
-      //  using multiple *could* really mess things up.
+      // So if we only see 1 april tag, we have *high* standard deviations -> lower confidence
       if (vision.getNumberOfAprilTags(limelightNumber) == 1) {
+        // But then we use the lookup table here to account for how far away the robot is from the april tag
+        // because if we are closer to the april tag, we are more confident in our position -> lower standard deviation
         double[] standardDeviations =
             oneAprilTagLookupTable.getLookupValue(distanceFromClosestAprilTag);
         swerveDrive.setPoseEstimatorVisionConfidence(
@@ -73,11 +68,10 @@ public void calculatePoseFromLimelight(Limelight limelightNumber) {
             standardDeviations[0], standardDeviations[1], standardDeviations[2]);
       }
 
+      // Adds the timestamped pose gotten from the limelights to our pose estimation
       swerveDrive.addPoseEstimatorVisionMeasurement(
           vision.getPoseFromAprilTags(limelightNumber),
           Timer.getFPGATimestamp() - vision.getLatencySeconds(limelightNumber));
     }
-
-    lastTimeStampSeconds = currentTimeStampSeconds;
   }
 }