Code review comments

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;
   }

src/main/java/frc/robot/elevator/ElevatorSubsystem.java

@@ -27,17 +27,19 @@ 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;
   private HomingState homingState = HomingState.PRE_MATCH_HOMING;
 
   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
 }

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);

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();

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;
   }