Clean up

src/main/java/frc/robot/Constants.java

@@ -1,7 +1,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.RobotBase;
-import frc.robot.utility.Alert;
+import frc.robot.utility.logging.Alert;
 
 /**
  * The Constants class provides a convenient place to hold robot-wide numerical or boolean

src/main/java/frc/robot/RobotContainer.java

@@ -36,11 +36,11 @@
 import frc.robot.subsystems.vision.AprilTagVision;
 import frc.robot.subsystems.vision.CameraIOSim;
 import frc.robot.subsystems.vision.VisionConstants;
-import frc.robot.utility.Alert;
-import frc.robot.utility.Alert.AlertType;
 import frc.robot.utility.OverrideSwitch;
 import frc.robot.utility.SpeedController;
 import frc.robot.utility.SpeedController.SpeedLevel;
+import frc.robot.utility.logging.Alert;
+import frc.robot.utility.logging.Alert.AlertType;
 import java.util.Optional;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -69,7 +69,7 @@ public class RobotContainer {
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     switch (Constants.getRobot()) {
-      case COMP_BOT:
+      case COMP_BOT, DEV_BOT:
         // Real robot, instantiate hardware IO implementations
         drive =
             new Drive(
@@ -82,19 +82,6 @@ public RobotContainer() {
         vision = new AprilTagVision();
         break;
 
-      case DEV_BOT:
-        // Real robot, instantiate hardware IO implementations
-        drive =
-            new Drive(
-                new GyroIOPigeon2(false),
-                new ModuleIOSparkMax(DriveConstants.FRONT_LEFT_MODULE_CONFIG),
-                new ModuleIOSparkMax(DriveConstants.FRONT_RIGHT_MODULE_CONFIG),
-                new ModuleIOSparkMax(DriveConstants.BACK_LEFT_MODULE_CONFIG),
-                new ModuleIOSparkMax(DriveConstants.BACK_RIGHT_MODULE_CONFIG));
-        flywheelExample = new Flywheel(new FlywheelIOSparkMax());
-        vision = new AprilTagVision();
-        break;
-
       case OLD_DEV_BOT:
         // Real robot, instantiate hardware IO implementations
         drive =
@@ -207,12 +194,18 @@ private void configureDriverControllerBindings() {
       final Trigger useFieldRelative =
           new Trigger(
               new OverrideSwitch(
-                  driverXbox.y(), "Field Relative", OverrideSwitch.Mode.TOGGLE, true));
+                  driverXbox.y(),
+                  OverrideSwitch.Mode.TOGGLE,
+                  true,
+                  (state) -> SmartDashboard.putBoolean("Field Relative", state)));
 
       final Trigger useAngleControlMode =
           new Trigger(
               new OverrideSwitch(
-                  driverXbox.rightBumper(), "Angle Driven", OverrideSwitch.Mode.HOLD, false));
+                  driverXbox.rightBumper(),
+                  OverrideSwitch.Mode.HOLD,
+                  false,
+                  (state) -> SmartDashboard.putBoolean("Angle Driven", state)));
 
       // Controllers
       final TeleopDriveController input =
@@ -452,9 +445,10 @@ public void initSmartDashboardOutputs() {
   }
 
   public void updateSmartDashboardOutputs() {
-    Pose2d pose = drive.getPose();
 
+    Pose2d pose = drive.getPose();
     ChassisSpeeds speeds = drive.getRobotSpeeds();
+
     SmartDashboard.putNumber("Heading Degrees", -pose.getRotation().getDegrees());
     SmartDashboard.putNumber(
         "Speed MPH", Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) * 2.2369);

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -4,8 +4,8 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants;
-import frc.robot.utility.FeedForwardConstants;
-import frc.robot.utility.PIDConstants;
+import frc.robot.utility.records.FeedForwardConstants;
+import frc.robot.utility.records.PIDConstants;
 import frc.robot.utility.swerve254util.ModuleLimits;
 
 /**

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -8,8 +8,8 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import frc.robot.utility.LoggedTunableNumber;
-import frc.robot.utility.LoggedTunableNumberGroup;
+import frc.robot.utility.logging.LoggedTunableNumber;
+import frc.robot.utility.logging.LoggedTunableNumberGroup;
 import org.littletonrobotics.junction.Logger;
 
 /**

src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java

@@ -10,8 +10,8 @@
 import frc.robot.RobotState;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveConstants;
-import frc.robot.utility.LoggedTunableNumber;
-import frc.robot.utility.LoggedTunableNumberGroup;
+import frc.robot.utility.logging.LoggedTunableNumber;
+import frc.robot.utility.logging.LoggedTunableNumberGroup;
 import frc.robot.utility.swerve254util.ModuleLimits;
 import org.littletonrobotics.junction.Logger;
 

src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java

@@ -7,8 +7,8 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.subsystems.drive.Drive;
-import frc.robot.utility.LoggedTunableNumber;
-import frc.robot.utility.LoggedTunableNumberGroup;
+import frc.robot.utility.logging.LoggedTunableNumber;
+import frc.robot.utility.logging.LoggedTunableNumberGroup;
 import java.util.Optional;
 import java.util.function.DoubleSupplier;
 

src/main/java/frc/robot/subsystems/vision/Camera.java

@@ -9,9 +9,9 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
-import frc.robot.utility.Alert;
-import frc.robot.utility.LoggedTunableNumber;
-import frc.robot.utility.LoggedTunableNumberGroup;
+import frc.robot.utility.logging.Alert;
+import frc.robot.utility.logging.LoggedTunableNumber;
+import frc.robot.utility.logging.LoggedTunableNumberGroup;
 import java.util.Arrays;
 import java.util.DoubleSummaryStatistics;
 import java.util.Optional;

src/main/java/frc/robot/utility/OverrideSwitch.java

@@ -1,16 +1,16 @@
 package frc.robot.utility;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
 
 /** Class for switching on and off features, implements BooleanSupplier */
 public class OverrideSwitch implements BooleanSupplier {
 
   private boolean value;
 
-  private final String name;
+  private final Consumer<Boolean> onUpdate;
 
   public static enum Mode {
     TOGGLE,
@@ -21,13 +21,12 @@ public static enum Mode {
    * Creates new Override switch
    *
    * @param trigger trigger to toggle state
-   * @param name name for smart dashboard
    * @param mode either toggle or hold
    * @param defaultState default state
+   * @param onUpdate runnable which takes in new state
    */
-  public OverrideSwitch(Trigger trigger, String name, Mode mode, boolean defaultState) {
-    this.name = name;
-
+  public OverrideSwitch(
+      Trigger trigger, Mode mode, boolean defaultState, Consumer<Boolean> onUpdate) {
     switch (mode) {
       case TOGGLE:
         trigger.onTrue(Commands.runOnce(this::toggle));
@@ -41,15 +40,14 @@ public OverrideSwitch(Trigger trigger, String name, Mode mode, boolean defaultSt
         break;
     }
 
+    this.onUpdate = onUpdate;
+
     set(defaultState);
   }
 
   private void set(boolean value) {
     this.value = value;
-
-    if (name != null) {
-      SmartDashboard.putBoolean(name, value);
-    }
+    onUpdate.accept(this.value);
   }
 
   public void toggle() {

src/main/java/frc/robot/utility/Alert.java → src/main/java/frc/robot/utility/logging/Alert.java

@@ -1,16 +1,14 @@
-package frc.robot.utility;
+package frc.robot.utility.logging;
 
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
-import java.util.Comparator;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
-import java.util.function.Predicate;
 
 /** Class for managing persistent alerts to be sent over NetworkTables. */
 public class Alert {
@@ -92,13 +90,10 @@ private static class SendableAlerts implements Sendable {
     public final List<Alert> alerts = new ArrayList<>();
 
     public String[] getStrings(AlertType type) {
-      Predicate<Alert> activeFilter = (Alert x) -> x.type == type && x.active;
-      Comparator<Alert> timeSorter =
-          (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime);
       return alerts.stream()
-          .filter(activeFilter)
-          .sorted(timeSorter)
-          .map((Alert a) -> a.text)
+          .filter(a -> a.type == type && a.active)
+          .sorted((a1, a2) -> Double.compare(a2.activeStartTime, a1.activeStartTime))
+          .map(a -> a.text)
           .toArray(String[]::new);
     }
 

src/main/java/frc/robot/utility/LoggedTunableNumber.java → src/main/java/frc/robot/utility/logging/LoggedTunableNumber.java

@@ -1,4 +1,4 @@
-package frc.robot.utility;
+package frc.robot.utility.logging;
 
 import frc.robot.Constants;
 import java.util.Arrays;

src/main/java/frc/robot/utility/LoggedTunableNumberGroup.java → src/main/java/frc/robot/utility/logging/LoggedTunableNumberGroup.java

@@ -1,4 +1,4 @@
-package frc.robot.utility;
+package frc.robot.utility.logging;
 
 public class LoggedTunableNumberGroup {
   private final String root;

src/main/java/frc/robot/utility/FeedForwardConstants.java → src/main/java/frc/robot/utility/records/FeedForwardConstants.java

@@ -1,4 +1,4 @@
-package frc.robot.utility;
+package frc.robot.utility.records;
 
 /** Store gains for simple motor feed forward controller */
 public record FeedForwardConstants(double kS, double kV, double kA) {

src/main/java/frc/robot/utility/PIDConstants.java → src/main/java/frc/robot/utility/records/PIDConstants.java

@@ -1,11 +1,14 @@
-package frc.robot.utility;
+package frc.robot.utility.records;
 
-/** Store gains in feed forward controller */
+/** Store gains in PID controller */
 public record PIDConstants(double kP, double kI, double kD) {
+
+  /** Proportional Controller */
   public PIDConstants(double kP) {
     this(kP, 0, 0);
   }
 
+  /** Proportional Derivative Controller */
   public PIDConstants(double kP, double kD) {
     this(kP, 0, kD);
   }