diff --git a/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java b/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java
index 2b6e8e2c..57c83859 100644
--- a/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java
+++ b/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java
@@ -21,6 +21,8 @@ public ConveyorSubsystem(TalonFX motor, DigitalInput sensor) {
 
     this.motor = motor;
     this.sensor = sensor;
+
+    // TODO: We don't actually set the motor config here
   }
 
   @Override
@@ -34,6 +36,7 @@ public void enabledPeriodic() {
 
   @Override
   public void robotPeriodic() {
+    // TODO: Move this logic to enabledPeriodic(). Stop using goalPercentage", just command the motor directly
     switch (goalState) {
       case IDLE:
         goalPercentage = 0.0;
@@ -56,6 +59,8 @@ public void robotPeriodic() {
     }
   }
 
+  // TODO: Logging
+
   public void setState(ConveyorState state) {
     goalState = state;
   }
diff --git a/src/main/java/frc/robot/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/elevator/ElevatorSubsystem.java
index 83494629..f07d286a 100644
--- a/src/main/java/frc/robot/elevator/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/elevator/ElevatorSubsystem.java
@@ -27,10 +27,11 @@ public class ElevatorSubsystem extends LifecycleSubsystem {
   private final LoggedDashboardNumber ntDistance =
       new LoggedDashboardNumber("Elevator/DistanceOverride", -1);
 
-  // TODO: Put in config
+  // TODO: Move this to config
   private static final double TOLERANCE = 0.0;
 
   // Homing
+  // TODO: This should be a variable, not a field
   double currentHeight = 0.0;
   private boolean preMatchHomingOccured = false;
   private double lowestSeenHeight = 0.0;
@@ -38,6 +39,7 @@ public class ElevatorSubsystem extends LifecycleSubsystem {
 
   private double goalHeight = ElevatorPositions.STOWED;
 
+  // TODO: This should be a variable, not a field
   private int slot = 0;
 
   public ElevatorSubsystem(TalonFX motor) {
@@ -114,19 +116,23 @@ public HomingState getHomingState() {
     return homingState;
   }
 
+  // TODO: Remove this method, pre-match homing is already on by default
   public void startPreMatchHoming() {
     homingState = HomingState.PRE_MATCH_HOMING;
   }
 
+  // TODO: Rename to atDistance() or atPosition()
   public boolean atGoal(double distance) {
     return Math.abs(getHeight() - distance) < TOLERANCE;
   }
 
+  // TODO: Mark as static
   // Tune the radius in inches later
   private double rotationsToInches(Rotation2d rotations) {
     return rotations.getRadians() * (RobotConfig.get().elevator().rotationsToDistance());
   }
 
+  // TODO: Mark as static
   private Rotation2d inchesToRotations(double inches) {
     return Rotation2d.fromRadians(inches / (RobotConfig.get().elevator().rotationsToDistance()));
   }
@@ -135,4 +141,6 @@ private static double clampHeight(double height) {
     return MathUtil.clamp(
         height, RobotConfig.get().elevator().minHeight(), RobotConfig.get().elevator().maxHeight());
   }
+
+  // TODO: Logging
 }
diff --git a/src/main/java/frc/robot/imu/ImuSubsystem.java b/src/main/java/frc/robot/imu/ImuSubsystem.java
index d834cdfa..acc6d209 100644
--- a/src/main/java/frc/robot/imu/ImuSubsystem.java
+++ b/src/main/java/frc/robot/imu/ImuSubsystem.java
@@ -17,7 +17,9 @@
 
 public class ImuSubsystem extends LifecycleSubsystem {
   private final Pigeon2 imu;
+  // TODO: Delete this field
   private Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.5);
+  // TODO: Don't mark as static
   private static final InterpolatingDoubleTreeMap distanceToAngleTolerance =
       new InterpolatingDoubleTreeMap();
 
@@ -53,6 +55,7 @@ public Rotation2d getRobotAngularVelocity() {
     return Rotation2d.fromDegrees(imu.getRate());
   }
 
+  // TODO: Remove this unused method
   public Rotation2d getRoll() {
     return Rotation2d.fromDegrees(imu.getRoll().getValue());
   }
@@ -73,14 +76,17 @@ public Rotation2d getAngleToleranceFromDistanceToSpeaker(double distance) {
             : Rotation2d.fromDegrees(distanceToAngleTolerance.get(distance));
   }
 
+  // TODO: Delete this, other subsystems should not need to worry about managing the IMU's tolerances. That is an IMU concern.
   public void setTolerance(Rotation2d tolerance) {
     TOLERANCE = tolerance;
   }
 
+  // TODO: Delete this, other subsystems should not need to worry about managing the IMU's tolerances. That is an IMU concern.
   public Rotation2d getTolerance() {
     return TOLERANCE;
   }
 
+  // TODO: This should just calculate the tolerance here and should not store it
   public boolean atAngle(Rotation2d angle, double distance) {
     setTolerance(getAngleToleranceFromDistanceToSpeaker(distance));
     return atAngle(angle);
diff --git a/src/main/java/frc/robot/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/shooter/ShooterSubsystem.java
index e2c70258..9037bcda 100644
--- a/src/main/java/frc/robot/shooter/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/shooter/ShooterSubsystem.java
@@ -30,6 +30,7 @@ public class ShooterSubsystem extends LifecycleSubsystem {
   private double goalRPM = 0;
   double usedGoalRPM = 0;
 
+  // TODO: Don't mark these as static
   private static final InterpolatingDoubleTreeMap speakerDistanceToRPM =
       new InterpolatingDoubleTreeMap();
   private static final InterpolatingDoubleTreeMap floorSpotDistanceToRPM =
@@ -51,6 +52,7 @@ public ShooterSubsystem(TalonFX leftMotor, TalonFX rightMotor) {
     rightMotor.getConfigurator().apply(RobotConfig.get().shooter().rightMotorConfig());
   }
 
+  // TODO: Move this code to the bottom of robotPeriodic()
   @Override
   public void enabledPeriodic() {
     double overrideRPM = ntRPM.get();
diff --git a/src/main/java/frc/robot/wrist/WristSubsystem.java b/src/main/java/frc/robot/wrist/WristSubsystem.java
index 0d2a241f..511f60e7 100644
--- a/src/main/java/frc/robot/wrist/WristSubsystem.java
+++ b/src/main/java/frc/robot/wrist/WristSubsystem.java
@@ -38,7 +38,9 @@ public class WristSubsystem extends LifecycleSubsystem {
       new InterpolatingDoubleTreeMap();
 
   private Rotation2d lowestSeenAngle = new Rotation2d();
+  // TODO: This should be a variable, not a field
   private int slot = 0;
+  // TODO: Delete this
   private Rotation2d TOLERANCE = Rotation2d.fromDegrees(5);
 
   private boolean preMatchHomingOccured = false;
@@ -158,7 +160,7 @@ public boolean atAngle(Rotation2d angle) {
     return Math.abs(angle.getDegrees() - getAngle().getDegrees()) < TOLERANCE.getDegrees();
   }
 
-  // convert distance to tolerance
+  // TODO: Rename this to atAngleForSpeaker()
   public boolean atAngle(Rotation2d angle, double distance) {
     setTolerance(getToleranceFromDistanceToSpeaker(distance));
     return atAngle(angle);
@@ -184,6 +186,7 @@ public Rotation2d getAngleFromDistanceToSpeaker(double distance) {
     return Rotation2d.fromDegrees(speakerDistanceToAngle.get(distance));
   }
 
+  // TODO: Make this a private method
   public Rotation2d getToleranceFromDistanceToSpeaker(double distance) {
     return distance > 8
         ? Rotation2d.fromDegrees(0.5)
@@ -192,6 +195,8 @@ public Rotation2d getToleranceFromDistanceToSpeaker(double distance) {
             : Rotation2d.fromDegrees(distanceToAngleTolerance.get(distance));
   }
 
+
+    // TODO: Delete this. The wrist tolerance should not be a concern of other subsystems.
   public void setTolerance(Rotation2d tolerance) {
     TOLERANCE = tolerance;
   }