diff --git a/src/main/java/frc/robot/classes/KeyValue.java b/src/main/java/frc/robot/classes/KeyValue.java
index f0918752..9afe48f0 100644
--- a/src/main/java/frc/robot/classes/KeyValue.java
+++ b/src/main/java/frc/robot/classes/KeyValue.java
@@ -1,22 +1,36 @@
 package frc.robot.classes;
 
 public class KeyValue<T> {
-  public String k;
-  public T v;
-  public int x;
-  public int y;
+    public String k;
+    public T v;
+    public int x;
+    public int y;
 
-  public KeyValue(String k, T v) {
-    this.k = k;
-    this.v = v;
-    this.x = -1;
-    this.y = -1;
-  }
+    /**
+     * Constructor for KeyValue class.
+     * 
+     * @param k - key
+     * @param v - value
+     */
+    public KeyValue(String k, T v) {
+        this.k = k;
+        this.v = v;
+        this.x = -1;
+        this.y = -1;
+    }
 
-  public KeyValue(String k, T v, int x, int y) {
-    this.k = k;
-    this.v = v;
-    this.x = x;
-    this.y = y;
-  }
+    /**
+     * Overloaded constructor for KeyValue class with x and y coordinates
+     * 
+     * @param k - key
+     * @param v - value
+     * @param x - x coordinate
+     * @param y - y coordinate
+     */
+    public KeyValue(String k, T v, int x, int y) {
+        this.k = k;
+        this.v = v;
+        this.x = x;
+        this.y = y;
+    }
 }
diff --git a/src/main/java/frc/robot/classes/Kinematics.java b/src/main/java/frc/robot/classes/Kinematics.java
index 99c2a08c..6d0cf10e 100644
--- a/src/main/java/frc/robot/classes/Kinematics.java
+++ b/src/main/java/frc/robot/classes/Kinematics.java
@@ -2,113 +2,156 @@
 
 import frc.robot.subsystems.Drivetrain;
 
+/**
+ * This class is used to calculate the current position of the robot based on
+ * encoder readings and current heading.
+ */
 public class Kinematics {
-  private Position2D m_currentPose;
-  private double m_previousLeftEncoderPosition;
-  private double m_previousRightEncoderPosition;
-  private double m_trackWidthFeet = Drivetrain.TRACK_WIDTH_FEET;
-  
-  public Kinematics(Position2D startingPosition) {
-    m_currentPose = startingPosition;
-    m_previousLeftEncoderPosition = 0;
-    m_previousRightEncoderPosition = 0;
-  }
-
-  // This function runs the kinematics caluclation,
-  // given the delta Psi to use instead fo calculating from encoders
-  public void calculatePosition(double leftEncoderPostion, double rightEncoderPosition, double heading) {
-    double currentHeading = 0.0d;
-
-    updateCurrentPose(leftEncoderPostion, rightEncoderPosition);
-
-    // Limit psi between Pi and -Pi
-    currentHeading = limitRadians(heading);
-
-    // Calculate new attitude angle in radians
-    m_currentPose.setHeadingRadians(currentHeading);
-  }
-
-  // This function runs the kinematics caluclation,
-  // calculating delta Psi based off of encoder distance traveled
-  public void calculatePosition(double leftEncoderPostion, double rightEncoderPosition) {
-    updateCurrentPose(leftEncoderPostion, rightEncoderPosition);
-  }
-
-  private void updateCurrentPose(double leftEncoderPostion, double rightEncoderPosition) {
-    double deltaLeft = 0.0d;
-    double deltaRight = 0.0d;
-    double deltaX = 0.0d;
-    double deltaY = 0.0d;
-    double deltaPsi = 0.0d;
-    double newPsi = 0.0d;
-
-    // Calculate distance traveled
-    deltaLeft = (leftEncoderPostion - m_previousLeftEncoderPosition);
-    deltaRight = (rightEncoderPosition - m_previousRightEncoderPosition);
-
-    // Save distance traveled
-    m_previousLeftEncoderPosition = leftEncoderPostion;
-    m_previousRightEncoderPosition = rightEncoderPosition;
-
-    // Use encoders to calculate heading
-    deltaPsi = (deltaRight - deltaLeft) / m_trackWidthFeet; // this is the width from center left wheel to center right
-                                                            // wheel
-
-    // Limit psi between Pi and -Pi
-    deltaPsi = limitRadians(deltaPsi);
-    newPsi = limitRadians(m_currentPose.getHeadingRadians() + deltaPsi);
-
-    // Calcualte new X, Y
-    deltaX = ((deltaLeft + deltaRight) / 2) * Math.cos(m_currentPose.getHeadingRadians()); // previous heading is in
-                                                                                           // radians
-    deltaY = ((deltaLeft + deltaRight) / 2) * Math.sin(m_currentPose.getHeadingRadians()); // previous heading is in
-                                                                                           // radians
-
-    // Update new position
-    m_currentPose.setX(m_currentPose.getX() + deltaX);
-    m_currentPose.setY(m_currentPose.getY() + deltaY);
-    m_currentPose.setHeadingRadians(newPsi);
-  }
-
-  public Position2D getPose() {
-    return m_currentPose;
-  }
-
-  public void setPose(Position2D pose) {
-    // Reset Post
-    m_currentPose = pose;
-  }
-
-  private double limitRadians(double radians) {
-    double retval = radians;
-
-    while (retval > Math.PI) {
-      retval -= 2 * Math.PI;
+    private Position2D m_currentPose;
+    private double m_previousLeftEncoderPosition;
+    private double m_previousRightEncoderPosition;
+    private double m_trackWidthFeet = Drivetrain.TRACK_WIDTH_FEET;
+
+    public Kinematics(Position2D startingPosition) {
+        m_currentPose = startingPosition;
+        m_previousLeftEncoderPosition = 0;
+        m_previousRightEncoderPosition = 0;
     }
 
-    while (retval < -Math.PI) {
-      retval += 2 * Math.PI;
+    /**
+     * Calculates the current position of the robot based on encoder readings and
+     * current heading.
+     * @param leftEncoderPosition  The current position of the left encoder.
+     * @param rightEncoderPosition The current position of the right encoder.
+     * @param heading              The current heading of the robot.
+     */
+    public void calculatePosition(double leftEncoderPostion, double rightEncoderPosition, double heading) {
+        double currentHeading = 0.0d;
+
+        updateCurrentPose(leftEncoderPostion, rightEncoderPosition);
+
+        // Limit psi between Pi and -Pi
+        currentHeading = limitRadians(heading);
+
+        // Calculate new attitude angle in radians
+        m_currentPose.setHeadingRadians(currentHeading);
+    }
+
+    /**
+     * This function updates the robot's current pose based off of encoder distance traveled
+     * @param leftEncoderPosition distance traveled by the left encoder
+     * @param rightEncoderPosition distance traveled by the right encoder
+     */
+    public void calculatePosition(double leftEncoderPostion, double rightEncoderPosition) {
+        updateCurrentPose(leftEncoderPostion, rightEncoderPosition);
+    }
+
+    /**
+     * Updates the current pose of the robot based on the left and right encoder positions.
+     * @param leftEncoderPosition the left encoder position in feet
+     * @param rightEncoderPosition the right encoder position in feet
+     */
+    private void updateCurrentPose(double leftEncoderPostion, double rightEncoderPosition) {
+        double deltaLeft = 0.0d;
+        double deltaRight = 0.0d;
+        double deltaX = 0.0d;
+        double deltaY = 0.0d;
+        double deltaPsi = 0.0d;
+        double newPsi = 0.0d;
+
+        // Calculate distance traveled
+        deltaLeft = (leftEncoderPostion - m_previousLeftEncoderPosition);
+        deltaRight = (rightEncoderPosition - m_previousRightEncoderPosition);
+
+        // Save distance traveled
+        m_previousLeftEncoderPosition = leftEncoderPostion;
+        m_previousRightEncoderPosition = rightEncoderPosition;
+
+        // Use encoders to calculate heading
+        deltaPsi = (deltaRight - deltaLeft) / m_trackWidthFeet; // this is the width from center left wheel to center
+                                                                // right
+                                                                // wheel
+
+        // Limit psi between Pi and -Pi
+        deltaPsi = limitRadians(deltaPsi);
+        newPsi = limitRadians(m_currentPose.getHeadingRadians() + deltaPsi);
+
+        // Calcualte new X, Y
+        deltaX = ((deltaLeft + deltaRight) / 2) * Math.cos(m_currentPose.getHeadingRadians()); // previous heading is in
+                                                                                                // radians
+        deltaY = ((deltaLeft + deltaRight) / 2) * Math.sin(m_currentPose.getHeadingRadians()); // previous heading is in
+                                                                                                // radians
+
+        // Update new position
+        m_currentPose.setX(m_currentPose.getX() + deltaX);
+        m_currentPose.setY(m_currentPose.getY() + deltaY);
+        m_currentPose.setHeadingRadians(newPsi);
+    }
+
+    /**
+     * Returns the current pose of the robot as a Position2D object, containing the
+     * X, Y, and heading of the robot.
+     * @return the current pose of the robot.
+     */
+    public Position2D getPose() {
+        return m_currentPose;
+    }
+
+    /**
+     * Sets the current pose of the robot.
+     * @param pose the new pose of the robot.
+     */
+    public void setPose(Position2D pose) {
+        // Reset Post
+        m_currentPose = pose;
+    }
+
+    /**
+     * Limits the angle between -Pi and Pi.
+     * @param radians the angle in radians.
+     * @return the limited angle in radians.
+     */
+    private double limitRadians(double radians) {
+        double retval = radians;
+
+        while (retval > Math.PI) {
+            retval -= 2 * Math.PI;
+        }
+
+        while (retval < -Math.PI) {
+            retval += 2 * Math.PI;
+        }
+
+        return retval;
+    }
+
+    /**
+     * Calculates the distance between the current pose and a checkpoint.
+     * @param checkpoint the checkpoint to calculate the distance to.
+     * @return the distance to the checkpoint.
+     */
+    public double distanceFromPose(Position2D checkpoint) {
+        double x1 = m_currentPose.getX();
+        double y1 = m_currentPose.getY();
+        double x2 = checkpoint.getX();
+        double y2 = checkpoint.getY();
+        double distance = Math.sqrt(Math.pow((x2 - x1), 2) + Math.pow((y2 - y1), 2));
+
+        return distance;
     }
 
-    return retval;
-  }
-
-  public double distanceFromPose(Position2D checkpoint) {
-    double x1 = m_currentPose.getX();
-    double y1 = m_currentPose.getY();
-    double x2 = checkpoint.getX();
-    double y2 = checkpoint.getY();
-    double distance = Math.sqrt(Math.pow((x2 - x1), 2) + Math.pow((y2 - y1), 2));
-
-    return distance;
-  }
-
-  public boolean atPose(Position2D checkpoint, double threshold) {
-    double distanceFromPose = distanceFromPose(checkpoint);
-    if (distanceFromPose <= threshold) {
-      return true;
-    } else {
-      return false;
+    /**
+     * Checks if the robot is at a checkpoint.
+     * @param checkpoint the checkpoint to check.
+     * @param threshold the threshold distance to check if the robot is at the checkpoint.
+     * @return true if the robot is at the checkpoint, false otherwise.
+     */
+    public boolean atPose(Position2D checkpoint, double threshold) {
+        double distanceFromPose = distanceFromPose(checkpoint);
+        if (distanceFromPose <= threshold) {
+            return true;
+        } else {
+            return false;
+        }
     }
-  }
 }
diff --git a/src/main/java/frc/robot/classes/Position2D.java b/src/main/java/frc/robot/classes/Position2D.java
index 2d0058e5..2632a334 100644
--- a/src/main/java/frc/robot/classes/Position2D.java
+++ b/src/main/java/frc/robot/classes/Position2D.java
@@ -1,45 +1,97 @@
 package frc.robot.classes;
 
+/**
+ * This class is used to represent a position on the field,
+ * or more generally, a position on a 2D plane, with an
+ * associated heading.
+ */
 public class Position2D {
-  private double m_x;
-  private double m_y;
-  private double m_heading; // In radians
-
-  public Position2D(double x, double y, double heading) {
-    m_x = x;
-    m_y = y;
-    m_heading = heading;
-  }
-
-  public void setX(double x) {
-    m_x = x;
-  }
-
-  public void setY(double y) {
-    m_y = y;
-  }
-
-  public void setHeadingDegrees(double heading) {
-    m_heading = Math.toRadians(heading);
-  }
-
-  public void setHeadingRadians(double heading) {
-    m_heading = heading;
-  }
-
-  public double getX() {
-    return m_x;
-  }
-
-  public double getY() {
-    return m_y;
-  }
-
-  public double getHeadingDegrees() {
-    return Math.toDegrees(m_heading);
-  }
-
-  public double getHeadingRadians() {
-    return m_heading;
-  }
+    private double m_x;
+    private double m_y;
+    private double m_heading; // In radians
+
+    /**
+     * Constructor for Position2D class.
+     * 
+     * @param x       x coordinate
+     * @param y       y coordinate
+     * @param heading heading in radians
+     */
+    public Position2D(double x, double y, double heading) {
+        m_x = x;
+        m_y = y;
+        m_heading = heading;
+    }
+
+    /**
+     * Sets the x coordinate of the Position.
+     * 
+     * @param x the x coordinate of the Position
+     */
+    public void setX(double x) {
+        m_x = x;
+    }
+
+    /**
+     * Sets the y coordinate of the Position.
+     * 
+     * @param y the y coordinate of the Position
+     */
+    public void setY(double y) {
+        m_y = y;
+    }
+
+    /**
+     * Sets the heading of the Position in degrees.
+     * 
+     * @param heading the heading of the Position in degrees
+     */
+    public void setHeadingDegrees(double heading) {
+        m_heading = Math.toRadians(heading);
+    }
+
+    /**
+     * Sets the heading of the Position in radians.
+     * 
+     * @param heading the heading of the Position in radians
+     */
+    public void setHeadingRadians(double heading) {
+        m_heading = heading;
+    }
+
+    /**
+     * Returns the x coordinate of the Position.
+     * 
+     * @return the x coordinate of the Position
+     */
+    public double getX() {
+        return m_x;
+    }
+
+    /**
+     * Returns the y coordinate of the Position.
+     * 
+     * @return the y coordinate of the Position
+     */
+    public double getY() {
+        return m_y;
+    }
+
+    /**
+     * Returns the heading of the Position in degrees.
+     * 
+     * @return the heading of the Position in degrees
+     */
+    public double getHeadingDegrees() {
+        return Math.toDegrees(m_heading);
+    }
+
+    /**
+     * Returns the heading of the Position in radians.
+     * 
+     * @return the heading of the Position in radians
+     */
+    public double getHeadingRadians() {
+        return m_heading;
+    }
 }
diff --git a/src/main/java/frc/robot/classes/SPIKE293Utils.java b/src/main/java/frc/robot/classes/SPIKE293Utils.java
index 0de5b62b..7117ad99 100644
--- a/src/main/java/frc/robot/classes/SPIKE293Utils.java
+++ b/src/main/java/frc/robot/classes/SPIKE293Utils.java
@@ -2,12 +2,17 @@
 
 import edu.wpi.first.math.MathUtil;
 
+/**
+ * This class contains utility methods for the SPIKE 293 robot, including
+ * methods for converting encoder values to distance and applying deadbands.
+ * PLEASE CONSIDER CREATING A 293 CONTROLLER CLASS FOR THE DEADBAND METHOD.
+ */
 public final class SPIKE293Utils {
 
     public static final double MAX_ENCODER_VELOCITY = 20743.0d;
 
     public static final double WHEEL_CIRCUMFERENCE_FEET = (4.0d / 12.0d) * Math.PI; // Wheel radius 4 in, converting
-                                                                                        // to feet
+                                                                                    // to feet
     public static final double SECONDS_TO_DECISEC = 1.0d / 10.0d;
     public static final double DECISEC_TO_SECONDS = 10.0d / 1.0d;
     public static final double GEARBOX_RATIO_TO_ONE = 9.52d;
@@ -18,12 +23,16 @@ public final class SPIKE293Utils {
     public static final double GEAR_RATIO = 1.0d;
     public static final double MINUTES_TO_DECISECONDS = 600.0d;
 
-
     private SPIKE293Utils() {
         throw new AssertionError("utility class");
     }
 
     /**
+     * Applies a deadband to the input value. If the input value is within the
+     * deadband, the output is zero. If the input value is outside the deadband, the
+     * output is the input value minus the deadband. If the input value is negative,
+     * the output is negative.
+     * Note: Consider moving this to a 293Controller class.
      * 
      * @param input
      * @param deadband
@@ -57,7 +66,6 @@ public static double controllerVelocityToFeetPerSec(double encoderUnits) {
      * Converts from feet per second to encoder edges per 100 milliseconds.
      * 
      * @param Speed in ft/s
-     * 
      * @return Drivetrain velocity in encoder units(edges per 100 milliseconds)
      */
     public static double feetPerSecToControllerVelocity(double feetPerSec) {
diff --git a/src/main/java/frc/robot/classes/SmoothControl.java b/src/main/java/frc/robot/classes/SmoothControl.java
index e1c7bd89..d5e93333 100644
--- a/src/main/java/frc/robot/classes/SmoothControl.java
+++ b/src/main/java/frc/robot/classes/SmoothControl.java
@@ -1,96 +1,104 @@
 package frc.robot.classes;
 
+/**
+ * This is an abstracted version of the Smooth Control algorithm. The class
+ * contains methods to calculate the required turn rate for the robot to follow
+ * a path. It is only used by certain other commands and the documentation is
+ * listed within this class.
+ */
 public class SmoothControl {
-  public static final double K1 = 1.0d;
-  public static final double K2 = 3.0d;
-
-  public SmoothControl() {
-  }
-
-  // Modification of equation 13, Calculates the omegaDesired (in radians)
-  // modified by velocity aggressivness given required turn rate
-  public double computeTurnRate(Position2D currentPose, Position2D targetPose, double givenMaxVelocity, boolean inReverse) {
-    double omegaDesired = 0.0d; 
-    double poseHeading = currentPose.getHeadingRadians(); // Convert heading to radians
-    double targetHeading = targetPose.getHeadingRadians();
-    double maxVelocity = givenMaxVelocity;
-
-    // Are we going in reverse? 
-    if(inReverse) {
-      // Subtract PI to the current and target headings to "trick" smooth control into thinking we're facing forwards
-      poseHeading += Math.PI;
-      targetHeading += Math.PI;
-
-      // The given velocity will be negative, we must make it positive
-      maxVelocity *= -1.0d;
+    public static final double K1 = 1.0d;
+    public static final double K2 = 3.0d;
+
+    public SmoothControl() {
     }
 
-    // Limit pose heading to be within -Pi and Pi
-    poseHeading = limitRadians(poseHeading); // poseHeading is now radians
-
-    // With the velocity and robot heading set appropriately, get the range
-    // and the vector orientation that runs from the robot to the target
-    double dx = targetPose.getX() - currentPose.getX();
-    double dy = targetPose.getY() - currentPose.getY();
-    double range = getRange(targetPose, currentPose); // distance in feet
-    double r_angle = Math.atan2(dy, dx); // vector heading in radians
-
-    if (range != 0.0) {
-      // Compute the angle between this vector and the desired orientation at the
-      // target
-      double thetaT = targetHeading - r_angle;
-      thetaT = limitRadians(thetaT); // bound this between -PI to PI
-
-    // Compute the angle between current robot heading and the vector from
-    // the robot to the target
-    double del_r = poseHeading - r_angle;
-    del_r = limitRadians(del_r); // bound this between -PI to PI
-
-    // Calculate k (equation 14)
-    double k = calculateK(range, thetaT, del_r);
-
-      // All set, now the equation for the angular rate!
-      omegaDesired = vGivenK(k, maxVelocity) * k;
-    } 
-
-    return (omegaDesired);
-  }
-
-  // Equation 14, Calculates the required turn rate
-  private double calculateK(double range, double theta, double delta) {
-    double retval = 0.0d;
-    if(0.0d != range) {
-      retval = (-(1 / range) * (K2 * (delta - Math.atan(-K1 * theta)) +
-      Math.sin(delta) * (1.0 + (K1 / (1.0 + Math.pow((K1 * theta), 2))))));
+    // Modification of equation 13, Calculates the omegaDesired (in radians)
+    // modified by velocity aggressiveness given required turn rate
+    public double computeTurnRate(Position2D currentPose, Position2D targetPose, double givenMaxVelocity,
+            boolean inReverse) {
+        double omegaDesired = 0.0d;
+        double poseHeading = currentPose.getHeadingRadians(); // Convert heading to radians
+        double targetHeading = targetPose.getHeadingRadians();
+        double maxVelocity = givenMaxVelocity;
+
+        // Are we going in reverse?
+        if (inReverse) {
+            // Add PI to the current and target headings to "trick" smooth control into
+            // thinking we're facing forwards
+            poseHeading += Math.PI;
+            targetHeading += Math.PI;
+
+            // The given velocity will be negative, we must make it positive
+            maxVelocity *= -1.0d;
+        }
+
+        // Limit pose heading to be within -Pi and Pi
+        poseHeading = limitRadians(poseHeading); // poseHeading is now radians
+
+        // With the velocity and robot heading set appropriately, get the range
+        // and the vector orientation that runs from the robot to the target
+        double dx = targetPose.getX() - currentPose.getX();
+        double dy = targetPose.getY() - currentPose.getY();
+        double range = getRange(targetPose, currentPose); // distance in feet
+        double r_angle = Math.atan2(dy, dx); // vector heading in radians
+
+        if (range != 0.0) {
+            // Compute the angle between this vector and the desired orientation at the
+            // target
+            double thetaT = targetHeading - r_angle;
+            thetaT = limitRadians(thetaT); // bound this between -PI to PI
+
+            // Compute the angle between current robot heading and the vector from
+            // the robot to the target
+            double del_r = poseHeading - r_angle;
+            del_r = limitRadians(del_r); // bound this between -PI to PI
+
+            // Calculate k (equation 14)
+            double k = calculateK(range, thetaT, del_r);
+
+            // All set, now the equation for the angular rate!
+            omegaDesired = vGivenK(k, maxVelocity) * k;
+        }
+
+        return (omegaDesired);
     }
-    return (retval);
-  }
-
-  // Equation 15, Calculates how close we are to maximum velocity based off of
-  // required turn rate
-  private double vGivenK(double k, double vMax) {
-    double beta = 0.4d;
-    double lambda = 2;
-    return (vMax / (1 + Math.abs(Math.pow((beta * k), lambda))));
-  }
-
-  public double limitRadians(double radians) {
-    double retval = radians;
-
-    while (retval > Math.PI) {
-      retval -= 2 * Math.PI;
+
+    // Equation 14, Calculates the required turn rate
+    private double calculateK(double range, double theta, double delta) {
+        double retval = 0.0d;
+        if (0.0d != range) {
+            retval = (-(1 / range) * (K2 * (delta - Math.atan(-K1 * theta)) +
+                    Math.sin(delta) * (1.0 + (K1 / (1.0 + Math.pow((K1 * theta), 2))))));
+        }
+        return (retval);
     }
 
-    while (retval < -Math.PI) {
-      retval += 2 * Math.PI;
+    // Equation 15, Calculates how close we are to maximum velocity based off of
+    // required turn rate
+    private double vGivenK(double k, double vMax) {
+        double beta = 0.4d;
+        double lambda = 2;
+        return (vMax / (1 + Math.abs(Math.pow((beta * k), lambda))));
     }
 
-    return retval;
-  }
+    public double limitRadians(double radians) {
+        double retval = radians;
+
+        while (retval > Math.PI) {
+            retval -= 2 * Math.PI;
+        }
+
+        while (retval < -Math.PI) {
+            retval += 2 * Math.PI;
+        }
 
-  public double getRange(Position2D poseA, Position2D poseB) {
-    double dx = poseB.getX() - poseA.getX();
-    double dy = poseB.getY() - poseA.getY();
-    return (Math.sqrt(dx * dx + dy * dy)); // distance in feet
-  }
+        return retval;
+    }
+
+    public double getRange(Position2D poseA, Position2D poseB) {
+        double dx = poseB.getX() - poseA.getX();
+        double dy = poseB.getY() - poseA.getY();
+        return (Math.sqrt(dx * dx + dy * dy)); // distance in feet
+    }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/classes/SpikeBoard.java b/src/main/java/frc/robot/classes/SpikeBoard.java
index 140f7bf0..aca99d9a 100644
--- a/src/main/java/frc/robot/classes/SpikeBoard.java
+++ b/src/main/java/frc/robot/classes/SpikeBoard.java
@@ -6,78 +6,82 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 
+/**
+ * This class is irrelevant and I don't feel like writing documentation for it.
+ * - Trevor
+ */
 public class SpikeBoard {
-  public ShuffleboardTab tab;
-  private HashMap<String, GenericEntry> entries = new HashMap<>();
+    public ShuffleboardTab tab;
+    private HashMap<String, GenericEntry> entries = new HashMap<>();
 
-  public SpikeBoard(String tabTitle, KeyValue... args) {
-    tab = Shuffleboard.getTab(tabTitle);
-    for (KeyValue arg : args) {
-      if (arg.y == -1 || arg.x == -1) {
-        entries.put(arg.k, (GenericEntry) tab.add(arg.k, arg.v).getEntry());
-      } else {
-        entries.put(arg.k, (GenericEntry) tab.add(arg.k, arg.v).withPosition(arg.x, arg.y).getEntry());
-      }
+    public SpikeBoard(String tabTitle, KeyValue... args) {
+        tab = Shuffleboard.getTab(tabTitle);
+        for (KeyValue arg : args) {
+            if (arg.y == -1 || arg.x == -1) {
+                entries.put(arg.k, (GenericEntry) tab.add(arg.k, arg.v).getEntry());
+            } else {
+                entries.put(arg.k, (GenericEntry) tab.add(arg.k, arg.v).withPosition(arg.x, arg.y).getEntry());
+            }
+        }
     }
-  }
 
-  public double getDouble(String key, double defaultVal) {
-    return entries.get(key).getDouble(defaultVal);
-  }
+    public double getDouble(String key, double defaultVal) {
+        return entries.get(key).getDouble(defaultVal);
+    }
 
-  public boolean setDouble(String key, double val) {
-    boolean retVal = true;
-    try {
-      retVal = entries.get(key).setDouble(val);
-    } catch (NullPointerException e) {
-      entries.put(key, (GenericEntry) tab.add(key, val).getEntry());
+    public boolean setDouble(String key, double val) {
+        boolean retVal = true;
+        try {
+            retVal = entries.get(key).setDouble(val);
+        } catch (NullPointerException e) {
+            entries.put(key, (GenericEntry) tab.add(key, val).getEntry());
+        }
+        return retVal;
     }
-    return retVal;
-  }
 
-  public int getInt(String key, int defaultVal) {
-    return Long.valueOf(entries.get(key).getInteger(defaultVal)).intValue();
-  }
+    public int getInt(String key, int defaultVal) {
+        return Long.valueOf(entries.get(key).getInteger(defaultVal)).intValue();
+    }
 
-  public boolean setInt(String key, int val) {
-    boolean retVal = true;
-    try {
-      retVal = entries.get(key).setInteger(val);
-    } catch (NullPointerException e) {
-      entries.put(key, (GenericEntry) tab.add(key, val).getEntry());
+    public boolean setInt(String key, int val) {
+        boolean retVal = true;
+        try {
+            retVal = entries.get(key).setInteger(val);
+        } catch (NullPointerException e) {
+            entries.put(key, (GenericEntry) tab.add(key, val).getEntry());
+        }
+        return retVal;
     }
-    return retVal;
-  }
 
-  public boolean setBoolean(String key, boolean val) {
-    boolean retVal = true;
-    try {
-      retVal = entries.get(key).setBoolean(val);
-    } catch (NullPointerException e) {
-      entries.put(key, (GenericEntry) tab.add(key, val).getEntry());
+    public boolean setBoolean(String key, boolean val) {
+        boolean retVal = true;
+        try {
+            retVal = entries.get(key).setBoolean(val);
+        } catch (NullPointerException e) {
+            entries.put(key, (GenericEntry) tab.add(key, val).getEntry());
+        }
+        return retVal;
     }
-    return retVal;
-  }
 
-  public boolean getBoolean(String key, boolean defaultVal) {
-    return entries.get(key).getBoolean(defaultVal);
-  }
+    public boolean getBoolean(String key, boolean defaultVal) {
+        return entries.get(key).getBoolean(defaultVal);
+    }
 
-  public boolean setString(String key, String val) {
-    boolean retVal = true;
-    try {
-      retVal = entries.get(key).setString(val);
-    } catch (NullPointerException e) {
-      entries.put(key, (GenericEntry) tab.add(key, val).getEntry());
+    public boolean setString(String key, String val) {
+        boolean retVal = true;
+        try {
+            retVal = entries.get(key).setString(val);
+        } catch (NullPointerException e) {
+            entries.put(key, (GenericEntry) tab.add(key, val).getEntry());
+        }
+        return retVal;
     }
-    return retVal;
-  }
 
-  public String getString(String key, String defaultVal) {
-    return entries.get(key).getString(defaultVal);
-  }
+    public String getString(String key, String defaultVal) {
+        return entries.get(key).getString(defaultVal);
+    }
 
-  public ShuffleboardTab getTab() {
-    return tab;
-  }
+    public ShuffleboardTab getTab() {
+        return tab;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/AdjustArm.java b/src/main/java/frc/robot/commands/AdjustArm.java
index 7d9089e0..5ec8b473 100644
--- a/src/main/java/frc/robot/commands/AdjustArm.java
+++ b/src/main/java/frc/robot/commands/AdjustArm.java
@@ -1,6 +1,5 @@
 package frc.robot.commands;
 
-// import edu.wpi.first.hal.simulation.SpiReadAutoReceiveBufferCallback;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.classes.SPIKE293Utils;
@@ -15,6 +14,13 @@ public class AdjustArm extends CommandBase {
 
     public double m_adjustArmDeadband;
 
+    /**
+     * Adjusts the arm based on controller joystick values.
+     * Controller input values range from -1 to 1.
+     * 
+     * @param givenArm        Arm subsystem
+     * @param givenController Operator Xbox Controller
+     */
     public AdjustArm(Arm givenArm, XboxController givenController) {
         m_arm = givenArm;
         m_operatorXboxController = givenController;
@@ -51,15 +57,4 @@ public void execute() {
         // Moves the arm
         m_arm.adjustPosition(anglePercent, extendPercent);
     }
-
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-    }
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
 }
diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java
index 66801934..bf61dc3c 100644
--- a/src/main/java/frc/robot/commands/ArcadeDrive.java
+++ b/src/main/java/frc/robot/commands/ArcadeDrive.java
@@ -1,13 +1,3 @@
-// RobotBuilder Version: 3.1
-//
-// This file was generated by RobotBuilder. It contains sections of
-// code that are automatically generated and assigned by robotbuilder.
-// These sections will be updated in the future when you export to
-// Java from RobotBuilder. Do not put any code or make any change in
-// the blocks indicating autogenerated code or it will be lost on an
-// update. Deleting the comments indicating the section will prevent
-// it from being updated in the future.
-
 package frc.robot.commands;
 
 import edu.wpi.first.math.MathUtil;
@@ -16,9 +6,6 @@
 import frc.robot.classes.SPIKE293Utils;
 import frc.robot.subsystems.Drivetrain;
 
-/**
- *
- */
 public class ArcadeDrive extends CommandBase {
     public static final double DEFAULT_FORZA_DEADBAND = 0.04;
     public static final double DEFAULT_ARCADE_JOY_DEADBAND = 0.04;
@@ -34,8 +21,17 @@ public class ArcadeDrive extends CommandBase {
     private double m_velocityLimitPercentage;
     private double m_turningLimitPercentage;
 
-    public ArcadeDrive(Drivetrain subsystem, XboxController xboxcontroller) {
-        m_drivetrain = subsystem;
+    /**
+     * Used to drive the robot using a single joystick. The joystick is used to
+     * turn the robot and to move the robot forward and backward. The left stick
+     * of the supplied Xbox controller is used for input. X axis is used for
+     * turning, and the Y axis is used for forward/backward movement.
+     *
+     * @param suppliedDrivetrain The drivetrain subsystem this command will run on.
+     * @param xboxcontroller     The Xbox controller to use for input.
+     */
+    public ArcadeDrive(Drivetrain suppliedDrivetrain, XboxController xboxcontroller) {
+        m_drivetrain = suppliedDrivetrain;
         addRequirements(m_drivetrain);
         m_xboxcontroller = xboxcontroller;
 
@@ -47,11 +43,6 @@ public ArcadeDrive(Drivetrain subsystem, XboxController xboxcontroller) {
         Drivetrain.getTab().setDouble("Max Turning Percentage", m_turningLimitPercentage);
     }
 
-    // Called when the command is initially scheduled.
-    @Override
-    public void initialize() {
-    }
-
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
@@ -96,12 +87,6 @@ public void end(boolean interrupted) {
         m_drivetrain.stop();
     }
 
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
     @Override
     public boolean runsWhenDisabled() {
         return false;
diff --git a/src/main/java/frc/robot/commands/AutoBalance.java b/src/main/java/frc/robot/commands/AutoBalance.java
index c26ceaa4..548b6195 100644
--- a/src/main/java/frc/robot/commands/AutoBalance.java
+++ b/src/main/java/frc/robot/commands/AutoBalance.java
@@ -34,6 +34,13 @@ public class AutoBalance extends CommandBase {
 
     private Drivetrain m_driveTrain;
 
+    /**
+     * Auto-balances the robot using PID control on the charge station.
+     * Using the pitch of the robot, the robot will balance itself. It will
+     * creep forward or backwards depending on the angle until it is balanced.
+     * 
+     * @param driveTrain the drivetrain subsystem
+     */
     public AutoBalance(Drivetrain driveTrain) {
 
         m_driveTrain = driveTrain;
@@ -56,7 +63,6 @@ public AutoBalance(Drivetrain driveTrain) {
      * @param front
      * @return the nearest number
      */
-
     public int CheckNearestNumber(int back, int num, int front) {
         int frontDiff = Math.abs(front - num);
         int backDiff = Math.abs(back - num);
diff --git a/src/main/java/frc/robot/commands/BumpDrive.java b/src/main/java/frc/robot/commands/BumpDrive.java
index 239ace61..7a9797f9 100644
--- a/src/main/java/frc/robot/commands/BumpDrive.java
+++ b/src/main/java/frc/robot/commands/BumpDrive.java
@@ -1,13 +1,3 @@
-// RobotBuilder Version: 3.1
-//
-// This file was generated by RobotBuilder. It contains sections of
-// code that are automatically generated and assigned by robotbuilder.
-// These sections will be updated in the future when you export to
-// Java from RobotBuilder. Do not put any code or make any change in
-// the blocks indicating autogenerated code or it will be lost on an
-// update. Deleting the comments indicating the section will prevent
-// it from being updated in the future.
-
 package frc.robot.commands;
 
 import edu.wpi.first.math.MathUtil;
@@ -21,6 +11,14 @@ public class BumpDrive extends CommandBase {
     private final Drivetrain m_drivetrain;
     private final double m_bumpPercent;
 
+    /**
+     * Uses the bumpers on the driver controller to turn the robot small amounts.
+     * This is used because it allows more accuracy when making small movements, as
+     * opposed to using the joystick.
+     * 
+     * @param subsystem
+     * @param bumpPercent
+     */
     public BumpDrive(Drivetrain subsystem, double bumpPercent) {
         m_drivetrain = subsystem;
         m_bumpPercent = bumpPercent;
@@ -28,11 +26,6 @@ public BumpDrive(Drivetrain subsystem, double bumpPercent) {
         addRequirements(m_drivetrain);
     }
 
-    // Called when the command is initially scheduled.
-    @Override
-    public void initialize() {
-    }
-
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
@@ -47,15 +40,4 @@ public void execute() {
     public void end(boolean interrupted) {
         m_drivetrain.stop();
     }
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    @Override
-    public boolean runsWhenDisabled() {
-        return false;
-    }
 }
diff --git a/src/main/java/frc/robot/commands/CalibrateExtender.java b/src/main/java/frc/robot/commands/CalibrateExtender.java
index 402f470d..df626223 100644
--- a/src/main/java/frc/robot/commands/CalibrateExtender.java
+++ b/src/main/java/frc/robot/commands/CalibrateExtender.java
@@ -6,10 +6,16 @@
 public class CalibrateExtender extends CommandBase {
     private final Arm arm;
 
+    /**
+     * Calibrates the extender by retracting it slowly until it hits the limit
+     * switch.
+     * 
+     * @param givenArm the arm subsystem
+     */
     public CalibrateExtender(Arm givenArm) {
         arm = givenArm;
-        
-        addRequirements(arm);                                                            
+
+        addRequirements(arm);
     }
 
     // Called when the command is initially scheduled.
@@ -17,23 +23,11 @@ public CalibrateExtender(Arm givenArm) {
     public void initialize() {
         // Invalidate the extender calibrations.
         arm.invalidateExtenderCalibration();
-    }                      
-
-    // Called every time the scheduler runs while the command is scheduled.
-    @Override
-    public void execute() {
-        /* Nothing to continuously do. (periodic will zero the arm for us) */ 
-        /* isFinished will check if the arm is still calibrating and end when done */
-    }
-
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
     }
 
     // Returns true when the command should end.
     @Override
     public boolean isFinished() {
-        return (true);
+        return arm.getExtenderCalibration();
     }
 }
diff --git a/src/main/java/frc/robot/commands/CalibratePivot.java b/src/main/java/frc/robot/commands/CalibratePivot.java
index a0b61a1d..13da4b35 100644
--- a/src/main/java/frc/robot/commands/CalibratePivot.java
+++ b/src/main/java/frc/robot/commands/CalibratePivot.java
@@ -6,10 +6,15 @@
 public class CalibratePivot extends CommandBase {
     private final Arm arm;
 
+    /**
+     * Calibrates the pivot by lowering it slowly until it hits the limit switch.
+     * 
+     * @param givenArm the arm subsystem
+     */
     public CalibratePivot(Arm givenArm) {
         arm = givenArm;
-        
-        addRequirements(arm);                                                            
+
+        addRequirements(arm);
     }
 
     // Called when the command is initially scheduled.
@@ -17,23 +22,11 @@ public CalibratePivot(Arm givenArm) {
     public void initialize() {
         // Invalidate the pivot calibrations.
         arm.invalidatePivotCalibration();
-    }                      
-
-    // Called every time the scheduler runs while the command is scheduled.
-    @Override
-    public void execute() {
-        /* Nothing to continuously do. (periodic will zero the arm for us) */ 
-        /* isFinished will check if the arm is still calibrating and end when done */
-    }
-
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
     }
 
     // Returns true when the command should end.
     @Override
     public boolean isFinished() {
-        return (true);
+        return arm.getPivotCalibration();
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/DriveBackwards.java b/src/main/java/frc/robot/commands/DriveBackwards.java
deleted file mode 100644
index f1905ba6..00000000
--- a/src/main/java/frc/robot/commands/DriveBackwards.java
+++ /dev/null
@@ -1,39 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.classes.Kinematics;
-import frc.robot.subsystems.Drivetrain;
-
-public class DriveBackwards extends CommandBase {
-    private Drivetrain m_drivetrain;
-    private Kinematics m_kinematics;
-    private double m_speed;
-    private double m_distance;
-
-    public DriveBackwards(Drivetrain drivetrain, Kinematics kinematics, double speed, double distance) {
-        m_drivetrain = drivetrain;
-        m_kinematics = kinematics;
-        m_speed = speed;
-
-        m_distance = distance;
-        addRequirements(drivetrain);
-    }
-
-    @Override
-    public void initialize() {
-    }
-
-    @Override
-    public void execute() {
-        m_drivetrain.percentDrive(m_speed, m_speed);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return Math.abs(m_kinematics.getPose().getX()) > Math.abs(m_distance);
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-    }
-}
diff --git a/src/main/java/frc/robot/commands/DriveTo.java b/src/main/java/frc/robot/commands/DriveTo.java
index f91d820f..493d1ea8 100644
--- a/src/main/java/frc/robot/commands/DriveTo.java
+++ b/src/main/java/frc/robot/commands/DriveTo.java
@@ -20,6 +20,16 @@ public class DriveTo extends CommandBase {
     private boolean m_inReverse = false;
     private boolean m_isDone = false;
 
+    /**
+     * Drive to a target pose at a max velocity. This takes in a target pose and a
+     * max velocity, as well as the kinematics of the robot and the drivetrain to
+     * control. If driving backwards, the max velocity should be negative.
+     * 
+     * @param targetPose  The target pose to drive to
+     * @param maxVelocity The max velocity to drive at (in feet per second)
+     * @param kinematics  The kinematics of the robot
+     * @param drivetrain  The drivetrain to control
+     */
     public DriveTo(Position2D targetPose, double maxVelocity, Kinematics kinematics, Drivetrain drivetrain) {
         m_targetPose = targetPose;
         m_maxVelocity = maxVelocity;
@@ -28,27 +38,25 @@ public DriveTo(Position2D targetPose, double maxVelocity, Kinematics kinematics,
         }
         m_kinematics = kinematics;
         m_drivetrain = drivetrain;
-        
+
         // This constructs smooth control
         m_smoothControl = new SmoothControl();
         addRequirements(m_drivetrain);
     }
 
-    @Override
-    public void initialize() {
-    }
-
     @Override
     public void execute() {
         final double trackWidthFeet = Drivetrain.TRACK_WIDTH_FEET;
         double vR = 0.0;
         double vL = 0.0;
         double omegaDesired = 0.0d;
-        Position2D currentPose = new Position2D(m_kinematics.getPose().getX(), m_kinematics.getPose().getY(), m_kinematics.getPose().getHeadingRadians());
+        Position2D currentPose = new Position2D(m_kinematics.getPose().getX(), m_kinematics.getPose().getY(),
+                m_kinematics.getPose().getHeadingRadians());
 
         // Have we reached the target?
         if ((trackWidthFeet * WITHIN_RANGE_MODIFIER) >= m_smoothControl.getRange(m_targetPose, currentPose)) {
-            // ending the command to allow the next sequential command with next point to run
+            // ending the command to allow the next sequential command with next point to
+            // run
             m_isDone = true;
         } else {
             // Compute turn rate in radians and update range
@@ -66,12 +74,13 @@ public void execute() {
             // Send vR and vL to velocity drive, units are in controller velocity
             m_drivetrain.velocityDrive(vL, vR);
         }
-        
+
         SmartDashboard.putNumber("Desired Left Velocity (ft/s)", vL);
         SmartDashboard.putNumber("Desired Right Velocity (ft/s)", vR);
         SmartDashboard.putNumber("Auto Range", m_smoothControl.getRange(m_targetPose, currentPose));
         SmartDashboard.putNumber("Auto Omega Desired (Degrees)", Math.toDegrees(omegaDesired));
-        SmartDashboard.putString("Target Pose", m_targetPose.getX() + ", " + m_targetPose.getY() + ", " + m_targetPose.getHeadingDegrees());
+        SmartDashboard.putString("Target Pose",
+                m_targetPose.getX() + ", " + m_targetPose.getY() + ", " + m_targetPose.getHeadingDegrees());
     }
 
     @Override
@@ -81,6 +90,9 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
+        // Consider making an abstracted DrivetrainCommandBase class that has a stop()
+        // that automatically stops the drivetrain when the command ends.
+        // This code is repeated in many commands.
         m_drivetrain.stop();
     }
 }
diff --git a/src/main/java/frc/robot/commands/DriveToBalance.java b/src/main/java/frc/robot/commands/DriveToBalance.java
index 1fea8f8f..a380cf83 100644
--- a/src/main/java/frc/robot/commands/DriveToBalance.java
+++ b/src/main/java/frc/robot/commands/DriveToBalance.java
@@ -8,6 +8,14 @@
 public class DriveToBalance extends CommandBase {
     private Drivetrain m_drivetrain;
 
+    /**
+     * This is deprecated and has been replaced with a specified distance driveTo
+     * command. Please use that instead. This command drives the robot forward until
+     * it reaches a certain angle. This is used to balance the robot on the charge
+     * station.
+     * 
+     * @param drivetrain
+     */
     public DriveToBalance(Drivetrain drivetrain) {
         m_drivetrain = drivetrain;
         addRequirements(drivetrain);
@@ -15,10 +23,6 @@ public DriveToBalance(Drivetrain drivetrain) {
         RobotContainer.getAutoBoard().setDouble("Balance Degrees", 85);
     }
 
-    @Override
-    public void initialize() {
-    }
-
     @Override
     public void execute() {
         double speed = RobotContainer.getAutoBoard().getDouble("Balance speed", 0);
@@ -38,8 +42,4 @@ public boolean isFinished() {
 
         return m_drivetrain.getGyroPitchDegrees() > (gyro);
     }
-
-    @Override
-    public void end(boolean interrupted) {
-    }
 }
diff --git a/src/main/java/frc/robot/commands/ForzaDrive.java b/src/main/java/frc/robot/commands/ForzaDrive.java
index 570e9d2e..b5337aed 100644
--- a/src/main/java/frc/robot/commands/ForzaDrive.java
+++ b/src/main/java/frc/robot/commands/ForzaDrive.java
@@ -17,7 +17,10 @@
 import frc.robot.subsystems.Drivetrain;
 
 /**
- *
+ * This command is used to drive the robot using the Forza control scheme, like
+ * the video game. Just like Forza, the robot will drive forward using right
+ * trigger and backwards using left trigger. The robot will turn using the left
+ * joystick.
  */
 public class ForzaDrive extends CommandBase {
     private final Drivetrain m_drivetrain;
@@ -48,12 +51,8 @@ public ForzaDrive(Drivetrain subsystem, XboxController xboxcontroller) {
         Drivetrain.getTab().setDouble("Max Turning Percentage", m_turningLimitPercentage);
     }
 
-    // Called when the command is initially scheduled.
-    @Override
-    public void initialize() {
-    }
-
     // Called every time the scheduler runs while the command is scheduled.
+    // Note: Abstract this out of this method, it's hard to read.
     @Override
     public void execute() {
         double turning;
@@ -111,15 +110,4 @@ public void execute() {
     public void end(boolean interrupted) {
         m_drivetrain.stop();
     }
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    @Override
-    public boolean runsWhenDisabled() {
-        return false;
-    }
 }
diff --git a/src/main/java/frc/robot/commands/MoveArm.java b/src/main/java/frc/robot/commands/MoveArm.java
index 1d2a6a0c..616cf08c 100644
--- a/src/main/java/frc/robot/commands/MoveArm.java
+++ b/src/main/java/frc/robot/commands/MoveArm.java
@@ -21,7 +21,8 @@ public class MoveArm extends CommandBase {
 
     public static final double SUBSTATION_X = 35.07279481d;
     public static final double SUBSTATION_Y = -1.658797242d;
-    public static final double SUBSTATION_R_INCHES = Math.sqrt(Math.pow(SUBSTATION_X, 2.0d) + Math.pow(SUBSTATION_Y, 2.0d));
+    public static final double SUBSTATION_R_INCHES = Math
+            .sqrt(Math.pow(SUBSTATION_X, 2.0d) + Math.pow(SUBSTATION_Y, 2.0d));
     public static final double SUBSTATION_ANGLE = Math.atan2(SUBSTATION_Y, SUBSTATION_X);
 
     public enum Node {
@@ -35,6 +36,13 @@ public enum Node {
     private final Arm m_arm;
     private final Node m_node;
 
+    /**
+     * Moves the arm to a given node height, high mid or low, or other arbitrary
+     * positions, using an enum.
+     * 
+     * @param givenArm
+     * @param node
+     */
     public MoveArm(Arm givenArm, Node node) {
         m_arm = givenArm;
         m_node = node;
@@ -42,11 +50,6 @@ public MoveArm(Arm givenArm, Node node) {
         addRequirements(m_arm);
     }
 
-    // Called when the command is initially scheduled.
-    @Override
-    public void initialize() {
-    }
-
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
@@ -68,14 +71,11 @@ public void execute() {
         }
     }
 
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-    }
-
     // Returns true when the command should end.
     @Override
     public boolean isFinished() {
+        // Consider making a SingleCommandBase class that has a default isFinished()
+        // method that returns true. This code is repeated in many commands.
         return true;
     }
 }
diff --git a/src/main/java/frc/robot/commands/MoveClaw.java b/src/main/java/frc/robot/commands/MoveClaw.java
index 2c82fada..3cc184ee 100644
--- a/src/main/java/frc/robot/commands/MoveClaw.java
+++ b/src/main/java/frc/robot/commands/MoveClaw.java
@@ -19,11 +19,6 @@ public MoveClaw(Claw givenClaw, XboxController givenController) {
         Claw.getTab().setDouble("Claw Trigger Deadband", m_triggerDeadband);
     }
 
-    // Called when the command is initially scheduled.
-    @Override
-    public void initialize() {
-    }
-
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
@@ -42,16 +37,4 @@ public void execute() {
             m_claw.percentClaw(-leftTrigger, 7.0d);
         }
     }
-
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-
-    }
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
 }
diff --git a/src/main/java/frc/robot/commands/RCFDrive.java b/src/main/java/frc/robot/commands/RCFDrive.java
index f513e59b..54b5fb1b 100644
--- a/src/main/java/frc/robot/commands/RCFDrive.java
+++ b/src/main/java/frc/robot/commands/RCFDrive.java
@@ -27,12 +27,22 @@ public class RCFDrive extends CommandBase {
     private double m_velocityLimitPercentage;
     private double m_turningLimitPercentage;
 
-    private final double DEFAULT_MAX_VELOCITY_PERCENTAGE = 0.85;
-    private final double DEFAULT_MAX_TURNING_SPEED = 0.55d;
-    private final double DEFAULT_RCF_JOY_DEADBAND = 0.04;
-
-    public RCFDrive(Drivetrain subsystem, XboxController xboxcontroller) {
-        m_drivetrain = subsystem;
+    private static final double DEFAULT_MAX_VELOCITY_PERCENTAGE = 0.85;
+    private static final double DEFAULT_MAX_TURNING_SPEED = 0.55d;
+    private static final double DEFAULT_RCF_JOY_DEADBAND = 0.04;
+
+    /**
+     * RCFDrive, short for Remote Control Flight Drive, is a control scheme that
+     * allows the driver to control the robot using two joysticks. Similarly to
+     * arcadeDrive, RCFDrive uses joysticks for speed and turning. However, the
+     * joysticks are split. The left stick Y axis is used for forward and backward
+     * speed, and the right stick X axis is used for turning.
+     * 
+     * @param givenDrivetrain The drivetrain to control
+     * @param xboxcontroller  The controller to use
+     */
+    public RCFDrive(Drivetrain givenDrivetrain, XboxController xboxcontroller) {
+        m_drivetrain = givenDrivetrain;
         addRequirements(m_drivetrain);
         m_xboxcontroller = xboxcontroller;
 
diff --git a/src/main/java/frc/robot/commands/ResetKinematics.java b/src/main/java/frc/robot/commands/ResetKinematics.java
index db26f860..f8482c6a 100644
--- a/src/main/java/frc/robot/commands/ResetKinematics.java
+++ b/src/main/java/frc/robot/commands/ResetKinematics.java
@@ -11,26 +11,27 @@ public class ResetKinematics extends CommandBase {
     private Position2D m_resetPosition;
     private Kinematics m_kinematics;
 
+    /**
+     * Reset the kinematics to a starting pose. This takes in a starting pose, as
+     * well as the kinematics of the robot and the drivetrain to control. This is
+     * useful for resetting the robot's position after a reset.
+     * 
+     * @param startingPose
+     * @param drivetrain
+     * @param kinematics
+     */
     public ResetKinematics(Position2D startingPose, Drivetrain drivetrain, Kinematics kinematics) {
         m_drivetrain = drivetrain;
         m_resetPosition = startingPose;
         m_kinematics = kinematics;
     }
 
-    @Override
-    public void initialize() {
-    }
-
     @Override
     public void execute() {
         m_kinematics.setPose(m_resetPosition);
         m_drivetrain.resetGyro(m_resetPosition.getHeadingDegrees());
     }
 
-    @Override
-    public void end(boolean interrupted) {
-    }
-
     @Override
     public boolean isFinished() {
         return true;
diff --git a/src/main/java/frc/robot/commands/Rotate.java b/src/main/java/frc/robot/commands/Rotate.java
index 8ba3acda..10869fb2 100644
--- a/src/main/java/frc/robot/commands/Rotate.java
+++ b/src/main/java/frc/robot/commands/Rotate.java
@@ -17,6 +17,14 @@ public class Rotate extends CommandBase {
     private double m_vI = 0.000;
     private double m_vD = 0.06;
 
+    /**
+     * Rotates the robot to a target angle. This takes in a target angle, as well as
+     * the drivetrain to control. Finishes when the robot is within 20 degrees of
+     * the target angle.
+     * 
+     * @param drivetrain
+     * @param targetDegrees
+     */
     public Rotate(Drivetrain drivetrain, double targetDegrees) {
         m_drivetrain = drivetrain;
         m_targetDegrees = targetDegrees;
@@ -35,7 +43,6 @@ public void initialize() {
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
-        double percentOutput = 0.0d;
         m_error = Math.abs(m_drivetrain.getGyroYawDegrees() - Math.abs(initalAngle - m_targetDegrees));
 
         m_change = m_error - m_lastError;
@@ -46,7 +53,7 @@ public void execute() {
             m_errorIntegral += m_error;
         }
 
-        percentOutput = (m_vP * m_error) + (m_vI * m_errorIntegral) + (m_vD * m_change);
+        double percentOutput = (m_vP * m_error) + (m_vI * m_errorIntegral) + (m_vD * m_change);
         percentOutput *= 0.4;
         clampPercentOutput(percentOutput);
 
@@ -55,11 +62,6 @@ public void execute() {
         m_drivetrain.percentDrive(-percentOutput, percentOutput);
     }
 
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-    }
-
     // Returns false when the command should end.
     @Override
     public boolean isFinished() {
diff --git a/src/main/java/frc/robot/commands/SequentialAutoCommand.java b/src/main/java/frc/robot/commands/SequentialAutoCommand.java
index bc416f8e..d5a20e32 100644
--- a/src/main/java/frc/robot/commands/SequentialAutoCommand.java
+++ b/src/main/java/frc/robot/commands/SequentialAutoCommand.java
@@ -1,7 +1,6 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.RobotContainer;
 import frc.robot.classes.Kinematics;
@@ -12,184 +11,174 @@
 import frc.robot.subsystems.Targeting;
 
 public class SequentialAutoCommand extends SequentialCommandGroup {
-    public static enum StartPositions {
+    public enum StartPositions {
+        /**
+         * The robot will not move during autonomous. This is the default auto and will
+         * earn 0 points.
+         */
         DONT_MOVE,
+        /**
+         * The robot will drive backwards for 10 feet. This has the potential to earn 3
+         * points, for mobility.
+         */
         DRIVE_BACKWARD,
+        /**
+         * The robot should start on the left side of the field, based on driver POV.
+         * The robot should face the scoring grid and align with the apriltag and should
+         * be preloaded with a cube. The robot will lift the arm, score a cube, and then
+         * leave the community. This auto can consistently earn 9 points.
+         */
         LEFT_SIDE_SCORE,
-        CENTER_ENGAGE,
+        /**
+         * The robot should start on the right side of the field, based on driver POV.
+         * The robot should face the scoring grid and align with the apriltag and should
+         * be preloaded with a cube. The robot will lift the arm, score a cube, and then
+         * leave the community. This auto can consistently earn 9 points.
+         */
         RIGHT_SIDE_SCORE,
-		SCORE_DONT_MOVE,
-		SCORE_AND_ENGAGE
-	}
-
-	private StartPositions m_startPosition;
-	private Drivetrain m_drivetrain;
-	private Kinematics m_kinematics;
-	private Arm m_arm;
-	private Claw m_claw;
-
-	public SequentialAutoCommand(Drivetrain drivetrain, Arm arm, Claw claw, Kinematics kinematics, Targeting targeting,
-			StartPositions startPosition) {
-		m_drivetrain = drivetrain;
-		m_arm = arm;
-		m_claw = claw;
-		m_kinematics = kinematics;
-		m_startPosition = startPosition;
-
-		RobotContainer.getAutoBoard().setBoolean("AutoDone", false);
-
-		switch (m_startPosition) { // Changes the robot path based on the starting position of the robot
-			case SCORE_DONT_MOVE:
-				// FACE SCORING GRID
-				resetKinematics();
-				score();
-				addCommands(
-					// Drive backwards
-					new DriveTo(new Position2D(-0.5, 0, Math.toRadians(0)), -1.5d, m_kinematics, m_drivetrain),
-					// Lower arm
-					new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES)
-				);
-				break;
-
-			case LEFT_SIDE_SCORE:
-				// FACE SCORING GRID
-				resetKinematics();
-				score();
-				addCommands(
-					// Drive backwards
-					new DriveTo(new Position2D(-0.5, -0.5, Math.toRadians(30)), -1.5d, m_kinematics, m_drivetrain),
-					// Drive backwards and lower arm
-					new ParallelCommandGroup(
-						new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES),
-						new DriveTo(new Position2D(-10, -2, Math.toRadians(0)), -5.0d, m_kinematics, m_drivetrain)
-					)
-				);
-				break;
-
-			case CENTER_ENGAGE:
-				// FACE SCORING GRID
-				resetKinematics();
-				chargeStationCenter();
-				break;
-
-			case SCORE_AND_ENGAGE:
-				// FACE SCORING GRID
-				resetKinematics();
-				score();
-				addCommands(
-					// Drive backwards
-					new DriveTo(new Position2D(-0.5, 0, Math.toRadians(0)), -2.0d, m_kinematics, m_drivetrain),
-					// Lower arm
-					new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES),
-					// Drive backwards (~3 feet)
-					new DriveTo(new Position2D(-6, 0, Math.toRadians(0)), -3.5d, m_kinematics, m_drivetrain),
-					// Autobalance
-					new AutoBalance(m_drivetrain)
-				);
-				break;
-
-			case RIGHT_SIDE_SCORE:
-				// FACE SCORING GRID
-				resetKinematics();
-				score();
-				addCommands(
-					// Drive backwards
-					new DriveTo(new Position2D(-0.5, 0.5, Math.toRadians(-30)), -1.5d, m_kinematics, m_drivetrain),
-					// Drive backwards and lower arm
-					new ParallelCommandGroup(
-						new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES),
-						new DriveTo(new Position2D(-10, 2, Math.toRadians(0)), -5.0d, m_kinematics, m_drivetrain)
-					)
-				);
-				break;
-
-			case DRIVE_BACKWARD:
-				// FACE SCORING GRID
-				resetKinematics();
-				addCommands(
-					// Drive backwards
-					new DriveTo(new Position2D(-10, 0, Math.toRadians(0)), -5.0d, m_kinematics, m_drivetrain)
-				);
-				break;
-
-			default:
-				resetKinematics();
-				System.out.println("ERROR: Invalid autonomous starting position! [" + m_startPosition + "]");
-				break;
-		}
-
-		// Alert smart dashboard that autonomous is done
-		RobotContainer.getAutoBoard().setBoolean("AutoDone", true);
-	}
-
-	// Score a piece in the high node
-	public void score() {
-		addCommands(
-			new SetClawForTime(m_claw, 1.0d, 1.0d),
-			// Close claw
-			new SetClaw(m_claw, -1.0d, 10.0d),
-			// Raise arm
-			new SetArm(m_arm, MoveArm.HIGH_ANGLE, Arm.STOW_R_INCHES),
-			new Wait(1.0d),
-			// Extend arm
-			new SetArm(m_arm,  MoveArm.HIGH_ANGLE, MoveArm.HIGH_R_INCHES),
-			// Drive forward (~1 foot)
-			new DriveTo(new Position2D(2, 0, Math.toRadians(0)),2.0d, m_kinematics, m_drivetrain),
-			// Open claw
-			new SetClawForTime(m_claw, 1.0d, 10.0d),
-			new Wait(0.5d),
-			// Retract arm
-			new SetArm(m_arm, MoveArm.HIGH_ANGLE, Arm.STOW_R_INCHES)
-		);
-	}
-
-	private void resetKinematics() {
-		addCommands(
-			// Reset kinematics to the blue left position
-			new ResetKinematics(new Position2D(0, 0, Math.toRadians(0)), m_drivetrain, m_kinematics)
-		);
-	}
-
-	// FACE CHARGE STATION
-	private void chargeStationCenter() {
-		double firstSpeed = RobotContainer.getAutoBoard().getDouble("first speed", 0);
-		firstSpeed = MathUtil.clamp(firstSpeed, -.4, 0);
-		RobotContainer.getAutoBoard().setDouble("first speed", firstSpeed);
-		double firstDistance = RobotContainer.getAutoBoard().getDouble("first distance", 0);
-		firstDistance = MathUtil.clamp(firstDistance, 0, 10);
-		RobotContainer.getAutoBoard().setDouble("first distance", firstDistance);
-
-		double secondSpeed = RobotContainer.getAutoBoard().getDouble("second speed", 0);
-		secondSpeed = MathUtil.clamp(secondSpeed, -.4, 0);
-		RobotContainer.getAutoBoard().setDouble("second speed", secondSpeed);
-		double secondDistance = RobotContainer.getAutoBoard().getDouble("second distance", 0);
-		secondDistance = MathUtil.clamp(secondDistance, 0, 10);
-		RobotContainer.getAutoBoard().setDouble("second distance", secondDistance);
-
-		double thirdSpeed = RobotContainer.getAutoBoard().getDouble("third speed", 0);
-		thirdSpeed = MathUtil.clamp(thirdSpeed, 0, 0.4);
-		RobotContainer.getAutoBoard().setDouble("third speed", thirdSpeed);
-		double thirdDistance = RobotContainer.getAutoBoard().getDouble("third distance", 0);
-		thirdDistance = MathUtil.clamp(thirdDistance, -10, 10);
-		RobotContainer.getAutoBoard().setDouble("third distance", thirdDistance);
-
-		addCommands(
-			new ResetKinematics(new Position2D(0, 0, Math.toRadians(180)), m_drivetrain,
-					m_kinematics),
-			new DriveBackwards(m_drivetrain, m_kinematics, firstSpeed, firstDistance),
-			// new ResetKinematics(new Position2D(0, 0, Math.toRadians(180)), m_drivetrain,
-			// m_kinematics),
-			// new DriveBackwards(m_drivetrain, m_kinematics, -.15, 2),
-			new ResetKinematics(new Position2D(0, 0, Math.toRadians(180)), m_drivetrain,
-					m_kinematics),
-			new DriveBackwards(m_drivetrain, m_kinematics, secondSpeed, secondDistance),
-			// new Wait(1),
-			// Reset kinematics to the blue left position
-			new DriveToBalance(m_drivetrain),
-			new ResetKinematics(new Position2D(0, 0, Math.toRadians(180)), m_drivetrain,
-					m_kinematics),
-			new DriveBackwards(m_drivetrain, m_kinematics, thirdSpeed, thirdDistance),
-			new AutoBalance(m_drivetrain)
-		);
-	}
+        /**
+         * The robot should start a foot in front of the scoring grid. The robot should
+         * face the scoring grid and align with the apriltag and should be preloaded
+         * with a cube. This auto can consistently earn 6 points.
+         */
+        SCORE_DONT_MOVE,
+        /**
+         * The robot should start a foot in front of the center scoring grid. The robot
+         * should face the scoring grid and align with the apriltag and should be
+         * preloaded with a cube. The robot will lift the arm, drive forward, score a
+         * cube, and then balance on the charge station. This auto can consistently earn
+         * 18 points.
+         */
+        SCORE_AND_ENGAGE
+    }
+
+    private StartPositions m_startPosition;
+    private Drivetrain m_drivetrain;
+    private Kinematics m_kinematics;
+    private Arm m_arm;
+    private Claw m_claw;
+
+    /**
+     * Schedules the autonomous commands based on the starting position of the
+     * robot.
+     * 
+     * @param drivetrain    The drivetrain subsystem
+     * @param arm           The arm subsystem
+     * @param claw          The claw subsystem
+     * @param kinematics    The kinematics subsystem
+     * @param targeting     The targeting subsystem
+     * @param startPosition The starting position of the robot
+     */
+    public SequentialAutoCommand(Drivetrain drivetrain, Arm arm, Claw claw, Kinematics kinematics, Targeting targeting,
+            StartPositions startPosition) {
+        m_drivetrain = drivetrain;
+        m_arm = arm;
+        m_claw = claw;
+        m_kinematics = kinematics;
+        m_startPosition = startPosition;
+
+        RobotContainer.getAutoBoard().setBoolean("AutoDone", false);
+
+        switch (m_startPosition) { // Changes the robot path based on the starting position of the robot
+            case SCORE_DONT_MOVE:
+                // FACE SCORING GRID
+                resetKinematics();
+                score();
+                addCommands(
+                        // Drive backwards
+                        new DriveTo(new Position2D(-0.5, 0, Math.toRadians(0)), -1.5d, m_kinematics, m_drivetrain),
+                        // Lower arm
+                        new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES));
+                break;
+
+            case LEFT_SIDE_SCORE:
+                // FACE SCORING GRID
+                resetKinematics();
+                score();
+                addCommands(
+                        // Drive backwards
+                        new DriveTo(new Position2D(-0.5, -0.5, Math.toRadians(30)), -1.5d, m_kinematics, m_drivetrain),
+                        // Drive backwards and lower arm
+                        new ParallelCommandGroup(
+                                new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES),
+                                new DriveTo(new Position2D(-10, -2, Math.toRadians(0)), -5.0d, m_kinematics,
+                                        m_drivetrain)));
+                break;
+
+            case SCORE_AND_ENGAGE:
+                // FACE SCORING GRID
+                resetKinematics();
+                score();
+                addCommands(
+                        // Drive backwards
+                        new DriveTo(new Position2D(-0.5, 0, Math.toRadians(0)), -2.0d, m_kinematics, m_drivetrain),
+                        // Lower arm
+                        new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES),
+                        // Drive backwards (~3 feet)
+                        new DriveTo(new Position2D(-6, 0, Math.toRadians(0)), -3.5d, m_kinematics, m_drivetrain),
+                        // Autobalance
+                        new AutoBalance(m_drivetrain));
+                break;
+
+            case RIGHT_SIDE_SCORE:
+                // FACE SCORING GRID
+                resetKinematics();
+                score();
+                addCommands(
+                        // Drive backwards
+                        new DriveTo(new Position2D(-0.5, 0.5, Math.toRadians(-30)), -1.5d, m_kinematics, m_drivetrain),
+                        // Drive backwards and lower arm
+                        new ParallelCommandGroup(
+                                new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES),
+                                new DriveTo(new Position2D(-10, 2, Math.toRadians(0)), -5.0d, m_kinematics,
+                                        m_drivetrain)));
+                break;
+
+            case DRIVE_BACKWARD:
+                // FACE SCORING GRID
+                resetKinematics();
+                addCommands(
+                        // Drive backwards
+                        new DriveTo(new Position2D(-10, 0, Math.toRadians(0)), -5.0d, m_kinematics, m_drivetrain));
+                break;
+
+            case DONT_MOVE:
+                // Do nothing and don't throw an error
+                break;
+
+            default:
+                resetKinematics();
+                System.out.println("ERROR: Invalid autonomous starting position! [" + m_startPosition + "]");
+                break;
+        }
+
+        // Alert smart dashboard that autonomous is done
+        RobotContainer.getAutoBoard().setBoolean("AutoDone", true);
+    }
+
+    // Score a piece in the high node
+    public void score() {
+        addCommands(
+                new SetClawForTime(m_claw, 1.0d, 1.0d),
+                // Close claw
+                new SetClaw(m_claw, -1.0d, 10.0d),
+                // Raise arm
+                new SetArm(m_arm, MoveArm.HIGH_ANGLE, Arm.STOW_R_INCHES),
+                new Wait(0.5d),
+                // Extend arm
+                new SetArm(m_arm, MoveArm.HIGH_ANGLE, MoveArm.HIGH_R_INCHES),
+
+                new DriveTo(new Position2D(2, 0, Math.toRadians(0)), 2.0d, m_kinematics, m_drivetrain),
+                // Open claw
+                new SetClawForTime(m_claw, 1.0d, 10.0d),
+                new Wait(1.5d),
+                // Retract arm
+                new SetArm(m_arm, MoveArm.HIGH_ANGLE, Arm.STOW_R_INCHES));
+    }
+
+    private void resetKinematics() {
+        addCommands(
+                // Reset kinematics to the blue left position
+                new ResetKinematics(new Position2D(0, 0, Math.toRadians(0)), m_drivetrain, m_kinematics));
+    }
 }
diff --git a/src/main/java/frc/robot/commands/SetArm.java b/src/main/java/frc/robot/commands/SetArm.java
index 2103fab8..6ea3bc7a 100644
--- a/src/main/java/frc/robot/commands/SetArm.java
+++ b/src/main/java/frc/robot/commands/SetArm.java
@@ -12,7 +12,8 @@ public class SetArm extends CommandBase {
 
     /**
      * Creates a new SetArm. This command will move the arm to the given position.
-     * Really only used during autonomous.
+     * Really only used during autonomous. This command waits until the arm is in
+     * the correct position before finishing.
      * 
      * @param givenArm The arm subsystem
      * @param theta    The angle to set the arm to
@@ -23,11 +24,6 @@ public SetArm(Arm givenArm, double theta, double rInches) {
         m_theta = theta;
         m_rInches = rInches;
 
-        // // find the difference in theta and rInches from the current position
-        // // and calculate approximately how long it will take to get there
-        // double deltaTheta = Math.abs(m_theta - m_arm.getTheta()) / 180.0d;
-        // double deltaR = Math.abs(m_rInches - m_arm.getRInches()) / 24.0d;
-
         addRequirements(m_arm);
     }
 
@@ -37,21 +33,6 @@ public void initialize() {
         m_arm.setPosition(m_theta, m_rInches);
     }
 
-    // Called every time the scheduler runs while the command is scheduled.
-    @Override
-    public void execute() {
-    }
-
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-    }
-
-    /**
-     * Returns true when the arm is at the given position
-     * 
-     * @return true when the arm is at the given position
-     */
     @Override
     public boolean isFinished() {
         double currentPos = m_arm.getPivotEncoderUnits();
diff --git a/src/main/java/frc/robot/commands/SetClaw.java b/src/main/java/frc/robot/commands/SetClaw.java
index e9c54b03..68daa042 100644
--- a/src/main/java/frc/robot/commands/SetClaw.java
+++ b/src/main/java/frc/robot/commands/SetClaw.java
@@ -1,20 +1,20 @@
 package frc.robot.commands;
 
-// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Claw;
 
-
 public class SetClaw extends CommandBase {
     private final Claw m_claw;
     private final double m_percent;
     private final double m_power;
 
     /**
-     * Creates a new SetClaw, which sets the claw to a given percent speed and stops it at a given force threshold.
+     * Creates a new SetClaw, which sets the claw to a given percent speed and stops
+     * it at a given force threshold.
+     * 
      * @param givenClaw The claw subsystem
-     * @param percent The percent to set the claw to. -1 to close, 1 to open
-     * @param power The power to stop the claw at
+     * @param percent   The percent to set the claw to. -1 to close, 1 to open
+     * @param power     The power to stop the claw at
      */
     public SetClaw(Claw givenClaw, double percent, double power) {
         m_claw = givenClaw;
@@ -24,11 +24,6 @@ public SetClaw(Claw givenClaw, double percent, double power) {
         addRequirements(m_claw);
     }
 
-    // Called when the command is initially scheduled.
-    @Override
-    public void initialize() {
-    }
-
     /**
      * Closes or opens the claw at a given percent speed -1 to 1
      */
@@ -37,13 +32,9 @@ public void execute() {
         m_claw.percentClaw(m_percent, m_power);
     }
 
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-    }
-
     /**
      * Returns true when the claw is at the given power
+     * 
      * @return true when the claw is feeling a certain level of resistance
      */
     @Override
diff --git a/src/main/java/frc/robot/commands/SetClawForTime.java b/src/main/java/frc/robot/commands/SetClawForTime.java
index bba75d63..a8540435 100644
--- a/src/main/java/frc/robot/commands/SetClawForTime.java
+++ b/src/main/java/frc/robot/commands/SetClawForTime.java
@@ -1,20 +1,21 @@
 package frc.robot.commands;
 
-// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Claw;
 
-
 public class SetClawForTime extends CommandBase {
     private final Claw m_claw;
     private final double m_percent;
     private final Wait m_time;
 
     /**
-     * Creates a new SetClaw, which sets the claw to a given percent speed and stops it at a given force threshold.
+     * Creates a new SetClawForTime, which sets the claw to a given percent speed
+     * and stops it after a given time. This command will move the claw in the given
+     * direction for a given time.
+     * 
      * @param givenClaw The claw subsystem
-     * @param percent The percent to set the claw to. -1 to close, 1 to open
-     * @param power The power to stop the claw at
+     * @param percent   The percent to set the claw to. -1 to close, 1 to open
+     * @param power     The power to stop the claw at
      */
     public SetClawForTime(Claw givenClaw, double percent, double time) {
         m_claw = givenClaw;
@@ -27,6 +28,7 @@ public SetClawForTime(Claw givenClaw, double percent, double time) {
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
+        m_time.initialize();
     }
 
     /**
@@ -44,6 +46,7 @@ public void end(boolean interrupted) {
 
     /**
      * Returns true when the claw is at the given power
+     * 
      * @return true when the claw is feeling a certain level of resistance
      */
     @Override
diff --git a/src/main/java/frc/robot/commands/TrackTarget.java b/src/main/java/frc/robot/commands/TrackTarget.java
index 7f7b1e9e..403ccda8 100644
--- a/src/main/java/frc/robot/commands/TrackTarget.java
+++ b/src/main/java/frc/robot/commands/TrackTarget.java
@@ -9,6 +9,13 @@ public class TrackTarget extends CommandBase {
     private final Drivetrain m_drivetrain;
     private final Targeting m_targeting;
 
+    /**
+     * Tracks a reflective tape target and faces toward it, based on information
+     * provided by the limelight data stream.
+     * 
+     * @param drivetrain
+     * @param targeting
+     */
     public TrackTarget(Drivetrain drivetrain, Targeting targeting) {
         m_drivetrain = drivetrain;
         m_targeting = targeting;
@@ -18,14 +25,14 @@ public TrackTarget(Drivetrain drivetrain, Targeting targeting) {
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
+        m_targeting.controlLight(true);
     }
 
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
         m_targeting.controlLight(true);
-        m_drivetrain.rotateDegrees(90.0d);
-        // m_drivetrain.rotateDegrees(m_targeting.getAngleToTargetDegrees());
+        m_drivetrain.rotateDegrees(m_targeting.getAngleToTargetDegrees());
     }
 
     // Called once the command ends or is interrupted.
@@ -37,6 +44,6 @@ public void end(boolean interrupted) {
     // Returns false when the command should end.
     @Override
     public boolean isFinished() {
-        return false; //m_targeting.isTargeted();
+        return m_targeting.isTargeted();
     }
 }
diff --git a/src/main/java/frc/robot/commands/ZeroArm.java b/src/main/java/frc/robot/commands/ZeroArm.java
index 1b4516de..280f23cc 100644
--- a/src/main/java/frc/robot/commands/ZeroArm.java
+++ b/src/main/java/frc/robot/commands/ZeroArm.java
@@ -6,10 +6,15 @@
 public class ZeroArm extends CommandBase {
     private final Arm arm;
 
+    /**
+     * Zeroes the arm by calibrating the pivot and extender.
+     * 
+     * @param givenArm
+     */
     public ZeroArm(Arm givenArm) {
         arm = givenArm;
-        
-        addRequirements(arm);                                                            
+
+        addRequirements(arm);
     }
 
     // Called when the command is initially scheduled.
@@ -18,12 +23,12 @@ public void initialize() {
         // Invalidate both the extender and the pivot calibrations.
         arm.invalidateExtenderCalibration();
         arm.invalidatePivotCalibration();
-    }                      
+    }
 
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
-        /* Nothing to continuously do. (periodic will zero the arm for us) */ 
+        /* Nothing to continuously do. (periodic will zero the arm for us) */
         /* isFinished will check if the arm is still calibrating and end when done */
     }
 
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index c0ca191b..3f70f028 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -87,7 +87,9 @@ public class Arm extends SubsystemBase {
     private double m_pivotCommandedEncoderUnits;
     private double m_extensionCommandedEncoderUnits;
 
-    // Gear ratios
+    /**
+     * Represents the arm of the robot.
+     */
     public Arm() {
         // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
         pivotTalonFX = new WPI_TalonFX(PIVOT_TALON_FX_CAN_ID);
@@ -204,15 +206,16 @@ public void rotateTo(double radians) {
     }
 
     /**
-     * Get the current encoder units.
-     * 
-     * @return
+     * Get the current encoder units of the pivot motor.
      */
     public double getPivotEncoderUnits() {
         double encoderUnits = pivotTalonFX.getSelectedSensorPosition();
         return encoderUnits;
     }
 
+    /**
+     * Get the current encoder units of the extender motor.
+     */
     public double getExtenderEncoderUnits() {
         double encoderUnits = extenderTalonFX.getSelectedSensorPosition();
         return encoderUnits;
@@ -279,6 +282,12 @@ public void setPosition(double targetTheta, double targetRInches) {
         rInches = targetRInches;
     }
 
+    /**
+     * Adjusts the position of the arm by a given percentage of the maximum delta.
+     * 
+     * @param anglePercent  - percentage of the maximum angle delta
+     * @param extendPercent - percentage of the maximum extension delta
+     */
     public void adjustPosition(double anglePercent, double extendPercent) {
         theta += ARM_THETA_DELTA_MODIFIER * anglePercent;
         rInches += ARM_R_DELTA_MODIFIER * extendPercent;
@@ -292,27 +301,54 @@ public void stowArm() {
         theta = STOW_ANGLE;
     }
 
+    /**
+     * Sets the encoder units of the pivot motor.
+     */
     public void pivotSetEncoderUnits(int encoderUnits) {
         pivotTalonFX.setSelectedSensorPosition(encoderUnits);
     }
 
+    /**
+     * Sets the encoder units of the extender motor.
+     */
     public void extenderSetEncoderUnits(int encoderUnits) {
         extenderTalonFX.setSelectedSensorPosition(encoderUnits);
     }
 
+    /**
+     * Invalidates the calibration of the pivot motor, which will cause it to
+     * recalibrate.
+     */
     public void invalidatePivotCalibration() {
         isPivotCalibrated = false;
     }
 
+    /**
+     * Returns whether the pivot motor is calibrated.
+     */
+    public boolean getPivotCalibration() {
+        return isPivotCalibrated;
+    }
+
+    /**
+     * Invalidates the calibration of the extender motor, which will cause it to
+     * recalibrate.
+     */
     public void invalidateExtenderCalibration() {
         isExtenderCalibrated = false;
     }
 
-    /*
-     * Checks if the Extender motor is calibrated '
-     * Does nothing if isExtenderCalibrated is true
-     * If false, once the reverse Extender limt switch is tripped the Extender motor
-     * is stopped, and the position is set
+    /**
+     * Returns whether the extender motor is calibrated.
+     */
+    public boolean getExtenderCalibration() {
+        return isExtenderCalibrated;
+    }
+
+    /**
+     * Checks if the extender motor is calibrated and does nothing if
+     * isExtenderCalibrated is true. If false, once the reverse extender limit
+     * switch is tripped the extender motor is stopped, and the position is set
      */
     private void checkExtenderCalibration() {
         // Do we need to check for calibration?
@@ -333,11 +369,10 @@ private void checkExtenderCalibration() {
         }
     }
 
-    /*
-     * Checks if the pivot motor is calibrated '
-     * Does nothing if isPivotCalibrated is true
-     * If false, once the reverse pivot limt switch is tripped the pivot motor is
-     * stopped, and the position is set
+    /**
+     * Checks if the pivot motor is calibrated and does nothing if isPivotCalibrated
+     * is true. If false, once the reverse pivot limt switch is tripped the pivot
+     * motor is stopped, and the position is set
      */
     private void checkPivotCalibration() {
         // Do we need to check for calibration?
@@ -358,14 +393,23 @@ private void checkPivotCalibration() {
         }
     }
 
+    /**
+     * Returns whether the arm is calibrated.
+     */
     public boolean isCalibrated() {
         return ((true == isPivotCalibrated) && (true == isExtenderCalibrated));
     }
 
+    /**
+     * Returns the current angle of the arm in radians.
+     */
     public double getTheta() {
         return theta;
     }
 
+    /**
+     * Returns the current extension of the arm in inches.
+     */
     public double getRInches() {
         return rInches;
     }
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index b2992f06..dd9e27a1 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -22,6 +22,9 @@ public class Claw extends SubsystemBase {
     private final double CLAW_LIMIT_PERCENTAGE = 0.4;
     private static SpikeBoard clawTab;
 
+    /**
+     * Creates a new Claw object. This object controls the claw motor.
+     */
     public Claw() {
         // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
         clawTalonFX = new WPI_TalonFX(CLAW_TALON_FX_CAN_ID);
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 4158ada0..7de6d9a6 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -75,6 +75,12 @@ public class Drivetrain extends SubsystemBase {
 
     public static final double TRACK_WIDTH_FEET = 27.5d / 12.0d; // Track width is 27.5 inches
 
+    /**
+     * Creates a new Drivetrain. This is the main class for the drivetrain
+     * subsystem.
+     * 
+     * @param kinematics
+     */
     public Drivetrain(Kinematics kinematics) {
         // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
         leftTalonLead = new WPI_TalonFX(LEFT_LEAD_TALON_CAN_ID);
@@ -149,6 +155,12 @@ public Drivetrain(Kinematics kinematics) {
         SmartDashboard.putNumber("Velocity Limit Delta", m_velocityLimitDelta);
     }
 
+    /**
+     * Sets the mode of the motor controllers. This means that the motor controllers
+     * will be set to coast or brake mode.
+     * 
+     * @param nm
+     */
     public void setNeutralMode(NeutralMode nm) {
         rightTalonLead.setNeutralMode(nm);
         rightTalonFollower.setNeutralMode(nm);
@@ -219,11 +231,28 @@ public void periodic() {
         }
     }
 
+    /**
+     * Sets the motor controllers to the percent power. This is relatively dangerous
+     * and is subject to internal motor issues as well as wear and tear on the motor
+     * controllers. The problem with this is that 100% power on the motors may not
+     * be equal.
+     * 
+     * @param leftPercentage  a value between -1.0 and 1.0
+     * @param rightPercentage a value between -1.0 and 1.0
+     */
     public void percentDrive(double leftPercentage, double rightPercentage) {
         leftTalonLead.set(ControlMode.PercentOutput, leftPercentage);
         rightTalonLead.set(ControlMode.PercentOutput, rightPercentage);
     }
 
+    /**
+     * Sets the motor controllers to the velocity. This is the preferred method of
+     * driving the robot. This method will use the PID loop on the motor controllers
+     * to drive the robot at the desired velocity.
+     * 
+     * @param velocity a value in feet per second
+     * @param turning  an angle in radians
+     */
     public void arcadeDrive(double velocity, double turning) {
         // Convert turning and speed to left right encoder velocity
         double leftMotorOutput;
@@ -254,13 +283,25 @@ public void arcadeDrive(double velocity, double turning) {
         velocityDrive(leftMotorOutput * MAX_VELOCITY, rightMotorOutput * MAX_VELOCITY);
     }
 
+    /**
+     * Force-stops the motors. This is a hard stop and will not allow the robot to
+     * coast to a stop. This is potentially dangerous and should only be used in
+     * emergencies, or special cases.
+     */
     public void stop() {
         leftTalonLead.set(0);
         rightTalonLead.set(0);
     }
 
-    // Sets the motors to encoder units per desisec (100ms), uses the onboard motor
-    // PID
+    /**
+     * Sets the motor controllers to the velocity. This is the preferred method of
+     * driving the robot. This method will use the PID loop on the motor controllers
+     * to drive the robot at the desired velocity. This method will also limit the
+     * acceleration of the robot to prevent wheel slip.
+     * 
+     * @param newVelocityL a value in encoder units per 100ms
+     * @param newVelocityR a value in encoder units per 100ms
+     */
     public void velocityDrive(double newVelocityL, double newVelocityR) {
         double changeLeft = newVelocityL - m_velocitySetPointLeft;
         double changeRight = newVelocityR - m_velocitySetPointRight;
@@ -355,22 +396,49 @@ private void zeroDriveTrainEncoders() {
         rightTalonLead.setSelectedSensorPosition(0);
     }
 
+    /**
+     * returns the gyro heading in degrees
+     * 
+     * @return gyro heading in degrees
+     */
     public double getGyroFusedHeadingDegrees() {
         return (navX.getFusedHeading() * -1.0d);
     }
 
+    /**
+     * returns the gyro pitch in degrees
+     * 
+     * @return gyro pitch in degrees
+     */
     public double getGyroPitchDegrees() {
         return (navX.getPitch() * -1.0d);
     }
 
+    /**
+     * returns the gyro yaw in degrees
+     * 
+     * @return gyro yaw in degrees
+     */
     public double getGyroYawDegrees() {
         return (navX.getYaw() * -1.0d);
     }
 
+    /**
+     * returns the gyro heading in degrees
+     * 
+     * @return gyro heading in degrees
+     */
     public double getGyroHeadingDegrees() {
         return (navX.getAngle() * -1.0d);
     }
 
+    /**
+     * calibrates the gyro and resets the yaw, then resets the gyro to the given
+     * heading
+     * 
+     * @param gyro                 the gyro to calibrate
+     * @param startingAngleDegrees the angle to reset the gyro to
+     */
     public void setupGyro(AHRS gyro, double startingAngleDegrees) {
 
         System.out.println("Calibrating gyroscope.");
@@ -389,16 +457,28 @@ public void setupGyro(AHRS gyro, double startingAngleDegrees) {
         System.out.println("Calibrating gyroscope done.");
     }
 
+    /**
+     * resets the robot kinematics
+     */
     public void resetKinematics() {
         setupGyro(navX, 0.0d);
         zeroDriveTrainEncoders();
     }
 
+    /**
+     * resets the gyro to the given heading
+     * 
+     * @param headingDegrees the angle to reset the gyro to
+     */
     public void resetGyro(double headingDegrees) {
         setupGyro(navX, headingDegrees);
     }
 
-    // rotates robot according to give degress using arc length formula
+    /**
+     * rotates robot according to give degress using arc length formula
+     * 
+     * @param angle
+     */
     public void rotateDegrees(double angle) {
         double radians = Math.toRadians(angle);
         double arcLength = (radians * (TRACK_WIDTH_FEET / 2.0));
@@ -408,7 +488,12 @@ public void rotateDegrees(double angle) {
         positionControl(leftEncoderPosition - encoderTicks, rightEncoderPosition + encoderTicks);
     }
 
-    // sets left and right talons to given parameters
+    /**
+     * sets left and right talons to given parameters
+     * 
+     * @param posL
+     * @param posR
+     */
     public void positionControl(double posL, double posR) {
         leftTalonLead.selectProfileSlot(POSITION_PID_SLOT_ID, 0);
         rightTalonLead.selectProfileSlot(POSITION_PID_SLOT_ID, 0);
@@ -417,6 +502,11 @@ public void positionControl(double posL, double posR) {
         rightTalonLead.set(ControlMode.Position, posR);
     }
 
+    /**
+     * gets the motor error from the left talon motor
+     * 
+     * @return motor error
+     */
     public double getMotorError() {
         return leftTalonLead.getClosedLoopError(0);
     }
diff --git a/src/main/java/frc/robot/subsystems/Targeting.java b/src/main/java/frc/robot/subsystems/Targeting.java
index 153d9b0c..4471f8f1 100644
--- a/src/main/java/frc/robot/subsystems/Targeting.java
+++ b/src/main/java/frc/robot/subsystems/Targeting.java
@@ -20,13 +20,13 @@
  */
 public class Targeting extends SubsystemBase {
     public static final int DEFAULT_TARGET_RPM = 2000;
-    
+
     public static final int LIMELIGHT_LED_ON = 3;
     public static final int LIMELIGHT_LED_OFF = 1;
-    
+
     public static final double TARGET_ACQUIRED = 1.0;
     public static final double CONFIRMED_THRESHOLD = 0.2;
-    
+
     private NetworkTable m_limeData; // Data from limelight
     private NetworkTableEntry m_tAcquired; // t stands for target
     private NetworkTableEntry m_targetX; // x value of the target
@@ -34,6 +34,11 @@ public class Targeting extends SubsystemBase {
 
     private boolean m_isReadyToFire = false;
 
+    /**
+     * Creates a new Targeting class. This class is used to get data from the
+     * limelight and calculate the distance to the target. It also controls the LED
+     * on the limelight.
+     */
     public Targeting() {
         // Get limelight data from network table
         m_limeData = NetworkTableInstance.getDefault().getTable("limelight");
@@ -59,11 +64,20 @@ public void periodic() {
         SmartDashboard.putNumber("Distance from Target", calcDistance());
     }
 
+    /**
+     * A command used during FRC 2022
+     * 
+     * @return
+     */
     public boolean getIsReadyToFire() {
         return m_isReadyToFire;
     }
 
-    // Turns the LED on or off
+    /**
+     * Turns the limelight LED on or off
+     * 
+     * @param enabled
+     */
     public void controlLight(boolean enabled) {
         if (enabled) {
             m_limeData.getEntry("ledMode").setNumber(LIMELIGHT_LED_ON);
@@ -76,12 +90,12 @@ public double calcShooterRPM() {
         double retval = DEFAULT_TARGET_RPM;
         double ty = m_targetY.getDouble(0.0);
         if (m_tAcquired.getDouble(0.0) == TARGET_ACQUIRED) {
-            //retv  al = (-30.07 * ty) + 1690.42;
+            // retv al = (-30.07 * ty) + 1690.42;
             retval = (230 * Math.pow(Math.E, ((-0.237 * ty) - 1.5))) + 1680.48;
-            if(retval > 2900.0){
+            if (retval > 2900.0) {
                 retval = 2900.0;
             }
-            if(retval < 1600.0){
+            if (retval < 1600.0) {
                 retval = 1600.0;
             }
         }
@@ -89,6 +103,12 @@ public double calcShooterRPM() {
         return retval;
     }
 
+    /**
+     * Returns true if the limelight has a target and the target is within the
+     * CONFIRMED_THRESHOLD
+     * 
+     * @return
+     */
     public boolean isTargeted() {
         boolean targeted = false;
         double limeError = m_targetX.getDouble(0.0); // Get the error of the target X
@@ -100,6 +120,11 @@ public boolean isTargeted() {
         return targeted;
     }
 
+    /**
+     * Calculates the distance to the target using the limelight
+     * 
+     * @return
+     */
     public double calcDistance() {
         double targetOffsetAngle_Vertical = m_targetY.getDouble(0.0);
         double limelightMountAngleDegrees = 36.574;
@@ -114,11 +139,22 @@ public double calcDistance() {
         return distanceFromLimelightToGoalInches;
     }
 
+    /**
+     * Returns the angle to the target in degrees. This is the angle that the robot
+     * needs to turn to face the target.
+     * 
+     * @return
+     */
     public double getAngleToTargetDegrees() {
         return -1 * m_targetX.getDouble(0);
     }
 
-    public boolean hasTarget(){
+    /**
+     * Returns true if the limelight has a target
+     * 
+     * @return
+     */
+    public boolean hasTarget() {
         if (m_tAcquired.getDouble(0.0) == TARGET_ACQUIRED) {
             return true;
         }
diff --git a/src/main/java/frc/robot/subsystems/WriteToCSV.java b/src/main/java/frc/robot/subsystems/WriteToCSV.java
index dcb1e1ca..a2bf1c87 100644
--- a/src/main/java/frc/robot/subsystems/WriteToCSV.java
+++ b/src/main/java/frc/robot/subsystems/WriteToCSV.java
@@ -6,66 +6,66 @@
 import java.io.FileWriter;
 
 public class WriteToCSV extends SubsystemBase {
-	String m_Filename = "/home/lvuser/match_data.csv";
-	final boolean m_Append = false;
-	BufferedWriter m_File;
+    String m_Filename = "/home/lvuser/match_data.csv";
+    final boolean m_Append = false;
+    BufferedWriter m_File;
 
-	public WriteToCSV() {
-		String path = "/home/lvuser/";
-		String filename = "match_data-a";
-		System.out.println("Initializing log file.");
-		
-		try {
-			//Check if file exists
-			File tempFile = new File(String.format("%s%s.csv", path, filename));
+    public WriteToCSV() {
+        String path = "/home/lvuser/";
+        String filename = "match_data-a";
+        System.out.println("Initializing log file.");
+        
+        try {
+            //Check if file exists
+            File tempFile = new File(String.format("%s%s.csv", path, filename));
 
-			if(true == tempFile.exists())
-			{
-				filename = "match_data-b";
-			}
+            if(true == tempFile.exists())
+            {
+                filename = "match_data-b";
+            }
 
-			m_Filename = String.format("%s%s.csv", path, filename);
+            m_Filename = String.format("%s%s.csv", path, filename);
 
-			// Open File
-			FileWriter fstream = new FileWriter(m_Filename, m_Append);
-			m_File = new BufferedWriter(fstream);
+            // Open File
+            FileWriter fstream = new FileWriter(m_Filename, m_Append);
+            m_File = new BufferedWriter(fstream);
 
-			//Write the header
-			writeHeader();
+            //Write the header
+            writeHeader();
 
-			System.out.println("Opened log file at " + m_Filename);
-		} catch (Exception e) {
-			System.out.println("Failed to open log file. " + m_Filename);
-		}
-	}
+            System.out.println("Opened log file at " + m_Filename);
+        } catch (Exception e) {
+            System.out.println("Failed to open log file. " + m_Filename);
+        }
+    }
 
-	@Override
+    @Override
     public void periodic() {
         try {
-			m_File.flush();
-		} catch (Exception e) {
-			//Cannot flush
-		}
+            m_File.flush();
+        } catch (Exception e) {
+            //Cannot flush
+        }
     }
 
-	private void writeHeader() {
+    private void writeHeader() {
         String stringToWrite = "ID, Time, TriggerMotorSet, BeltMotorSet, RpmSet, CurrentRpm, LauncherReady, DistanceToTarget, AngleToTargetDeg, IsTargeted\n";
         writeToFile(stringToWrite);
     }
 
-	// Writes a string to the data file.
-	// Returns true if successful, false otherwise
-	public boolean writeToFile(String stringToWrite) {
-		boolean retval = true;
+    // Writes a string to the data file.
+    // Returns true if successful, false otherwise
+    public boolean writeToFile(String stringToWrite) {
+        boolean retval = true;
 
-		try {
-			// Write string to file
-			m_File.append(stringToWrite);
-		} catch (Exception e) {
-			// Failed to write
-			retval = false;
-		}
+        try {
+            // Write string to file
+            m_File.append(stringToWrite);
+        } catch (Exception e) {
+            // Failed to write
+            retval = false;
+        }
 
-		return retval;
-	}
+        return retval;
+    }
 }