diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java
index 21f4236..fdb3853 100644
--- a/src/main/java/frc4388/robot/Main.java
+++ b/src/main/java/frc4388/robot/Main.java
@@ -28,5 +28,4 @@ public final class Main {
   }
 }
 
-
-// hi ryan
\ No newline at end of file
+// hi ryan -aarav
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index db7bb1a..debf72c 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -66,6 +66,8 @@ public class Robot extends TimedRobot {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    // print odometry data to smart dashboard for debugging (if causing timeout errors, you can comment it)
     SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
     SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
     SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
@@ -84,9 +86,6 @@ public class Robot extends TimedRobot {
 
   @Override
   public void disabledPeriodic() {
-    // SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
-    // SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
-    // SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
   }
 
   /**
@@ -114,6 +113,7 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     LOGGER.fine("teleopInit()");
+    m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
@@ -130,8 +130,6 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    // m_robotContainer.getDriverController().updateInput();
-    // m_robotContainer.getOperatorController().updateInput();
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 203bcff..9440932 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -44,7 +44,7 @@ public class RobotContainer {
   private final RobotMap m_robotMap = new RobotMap();
 
   /* Subsystems */
-  private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
+  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
     m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
 
   private final TalonFX m_testMotor = new TalonFX(23);
@@ -115,17 +115,19 @@ public class RobotContainer {
    * Generate autonomous
    * @param maxVel max velocity for the path (null to override default value of 5.0)
    * @param maxAccel max acceleration for the path (null to override default value of 5.0)
-   * @param inputs strings (paths) or commands you want to run (in order)
-   * @return array of commands
+   * @param inputs strings (path names) or commands you want to run (in order)
+   * @return array of commands, which can then be processed in a command group
    */
   public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
 
+    // default vel and acc
     maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL);
     maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC);
 
     ArrayList<Command> commands = new ArrayList<Command>();
     commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
 
+    // pids controlling the path
     PIDController xController = SwerveDriveConstants.X_CONTROLLER;
     PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
     ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
@@ -134,7 +136,9 @@ public class RobotContainer {
     // parse input
     for (int i=0; i<inputs.length; i++) {
 
+      // if string, process as pathplanner trajectory
       if (inputs[i] instanceof String) {
+
         PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
         PathPlannerState initState = (PathPlannerState) traj.sample(0);
 
@@ -150,6 +154,7 @@ public class RobotContainer {
                           m_robotSwerveDrive));
       }
 
+      // if command, just add it to the array
       if (inputs[i] instanceof Command) {
         commands.add((Command) inputs[i]);
       }
@@ -185,10 +190,18 @@ public class RobotContainer {
     return m_driverXbox;
   }
 
+  /**
+   * Get odometry.
+   * @return Odometry
+   */
   public Pose2d getOdometry() {
     return m_robotSwerveDrive.getOdometry();
   }
 
+  /**
+   * Set odometry to given pose.
+   * @param pose Pose to set odometry to.
+   */
   public void resetOdometry(Pose2d pose) {
     m_robotSwerveDrive.resetOdometry(pose);
   }
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 1bd3fb9..681fcb9 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems;
 
+import java.util.ArrayList;
+
 import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
 
@@ -155,18 +157,14 @@ public class SwerveDrive extends SubsystemBase {
   
   @Override
   public void periodic() {
-    // System.err.println(m_gyro.getFusedHeading() +"    aaa");
+
     updateOdometry();
     // SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
-    // SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
+    SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
     // SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
     // SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
     // SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
 
-    // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
     // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
     super.periodic();
   }
@@ -207,10 +205,9 @@ public class SwerveDrive extends SubsystemBase {
   }
 
   /**
-   * Resets the odometry of the robot to (x=0, y=0, theta=0).
+   * Resets the odometry of the robot to the given pose.
    */
   public void resetOdometry(Pose2d pose) {
-    // m_odometry.resetPosition(pose, m_gyro.getRotation2d());
     m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
   }
 
@@ -222,12 +219,6 @@ public class SwerveDrive extends SubsystemBase {
                             modules[1].getState(), 
                             modules[2].getState(), 
                             modules[3].getState());
-    
-    // m_odometry.update( m_gyro.getRotation2d(), 
-    //                    modules[0].getState(),
-    //                    modules[1].getState(),
-    //                    modules[2].getState(),
-    //                    modules[3].getState());
   
       // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
       // a real robot, this must be calculated based either on latency or timestamps.
@@ -241,6 +232,9 @@ public class SwerveDrive extends SubsystemBase {
     rotTarget = new Rotation2d(0);
   }
 
+  /**
+   * Stop all four swerve modules.
+   */
   public void stopModules() {
     modules[0].stop();
     modules[1].stop();
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index bc3d6f2..fdd7ed1 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -62,7 +62,7 @@ public class SwerveModule extends SubsystemBase {
 
   private Rotation2d getAngle() {
     // Note: This assumes the CANCoders are setup with the default feedback coefficient
-    // and the sesnor value reports degrees.
+    // and the sensor value reports degrees.
     return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
   }
 
@@ -90,20 +90,26 @@ public class SwerveModule extends SubsystemBase {
 
 
     double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-    // driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+
+    // Add this line to correct for the slight drive movement the angle motor makes when turning in place. 
+    // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+    
     driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
   }
 
   /**
-   * Returns the current state of the module.
+   * Get current module state.
    *
-   * @return The current state of the module.
+   * @return The current state of the module in m/s.
    */
   public SwerveModuleState getState() {
     // return state;
     return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
   }
 
+  /**
+   * Stop the drive and steer motors of current module.
+   */
   public void stop() {
     driveMotor.set(0);
     angleMotor.set(0);