Added drivebaseIO and logging auto selector

src/main/java/frc/robot/Robot.java

@@ -4,19 +4,18 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
-import edu.wpi.first.wpilibj.PowerDistribution;
-import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
-import edu.wpi.first.wpilibj.livewindow.LiveWindow;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
  * each mode, as described in the TimedRobot documentation. If you change the name of this class or
@@ -42,19 +41,26 @@ public void robotInit() {
     Logger.getInstance().recordMetadata("ProjectName", "MyProject"); // Set a metadata value
 
     if (isReal()) {
-        Logger.getInstance().addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick
-        Logger.getInstance().addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
-        new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+      Logger.getInstance().addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick
+      Logger.getInstance().addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+      new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
     } else {
-        setUseTiming(false); // Run as fast as possible
-        String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
-        Logger.getInstance().setReplaySource(new WPILOGReader(logPath)); // Read replay log
-        Logger.getInstance().addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+      setUseTiming(false); // Run as fast as possible
+      String logPath =
+          LogFileUtil
+              .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
+      Logger.getInstance().setReplaySource(new WPILOGReader(logPath)); // Read replay log
+      Logger.getInstance()
+          .addDataReceiver(
+              new WPILOGWriter(
+                  LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
     }
 
-// Logger.getInstance().disableDeterministicTimestamps() // See "Deterministic Timestamps" in the "Understanding Data Flow" page
-Logger.getInstance().start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
-
+    // Logger.getInstance().disableDeterministicTimestamps() // See "Deterministic Timestamps" in
+    // the "Understanding Data Flow" page
+    Logger.getInstance()
+        .start(); // Start logging! No more data receivers, replay sources, or metadata values may
+    // be added.
 
     robotContainer = new RobotContainer();
 

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

@@ -13,7 +13,6 @@
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -50,6 +49,7 @@
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
+import frc.robot.subsystems.DrivebaseSubsystemIO;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorState;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -73,6 +73,7 @@
 import java.util.Map;
 import java.util.Optional;
 import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -83,9 +84,15 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
 
+  private final LoggedDashboardChooser<Command> autoSelector =
+      new LoggedDashboardChooser<>("Auto Routine");
+
   private final VisionSubsystem visionSubsystem = new VisionSubsystem();
 
-  private final DrivebaseSubsystem drivebaseSubsystem = new DrivebaseSubsystem(visionSubsystem);
+  private final DrivebaseSubsystemIO drivebaseIO = new DrivebaseSubsystemIO();
+
+  private final DrivebaseSubsystem drivebaseSubsystem =
+      new DrivebaseSubsystem(visionSubsystem, drivebaseIO);
 
   private final RGBSubsystem rgbSubsystem = new RGBSubsystem();
 
@@ -111,8 +118,6 @@ public class RobotContainer {
   private final CommandXboxController anthony = new CommandXboxController(0);
 
   /** the sendable chooser to select which auto to run. */
-  private final SendableChooser<Command> autoSelector = new SendableChooser<>();
-
   private GenericEntry autoDelay;
 
   private final ShuffleboardTab driverView = Shuffleboard.getTab("DriverView");
@@ -582,16 +587,14 @@ public void end(boolean interrupted) {
         new N3_1ConePlusMobilityEngage(
             5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
 
-    autoSelector.setDefaultOption(
+    autoSelector.addDefaultOption(
         "N3 1Cone + Mobility",
         new N3_1ConePlusMobility(
             4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
 
-    autoSelector.setDefaultOption(
-        "N6 1Cone",
-        new N6_1Cone(intakeSubsystem, elevatorSubsystem));
-
-                autoSelector.setDefaultOption(
+    autoSelector.addDefaultOption("N6 1Cone", new N6_1Cone(intakeSubsystem, elevatorSubsystem));
+
+    autoSelector.addDefaultOption(
         "N6 1Cone + Engage",
         new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
 
@@ -637,9 +640,7 @@ public void end(boolean interrupted) {
    */
   public Command getAutonomousCommand() {
     double delay = autoDelay.getDouble(0);
-    return delay == 0
-        ? autoSelector.getSelected()
-        : new WaitCommand(delay).andThen(autoSelector.getSelected());
+    return delay == 0 ? autoSelector.get() : new WaitCommand(delay).andThen(autoSelector.get());
   }
 
   /**

src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java

@@ -6,8 +6,6 @@
 
 import static frc.robot.Constants.Drive.*;
 
-import org.littletonrobotics.junction.Logger;
-
 import com.kauailabs.navx.frc.AHRS;
 import com.playingwithfusion.TimeOfFlight;
 import com.playingwithfusion.TimeOfFlight.RangingMode;
@@ -63,8 +61,9 @@ public class DrivebaseSubsystem extends SubsystemBase {
   // Not mission critical as it "technically" drives fine as of now; but I suspect this is a site
   // for future improvements
 
-  private final DriveIO io;
-  private final DriveIOInputsAutoLogged inputs = new DriveIOInputsAutoLogged();
+  private final DrivebaseSubsystemIO io;
+  private final DrivebaseSubsystemIOInputsAutoLogged inputs =
+      new DrivebaseSubsystemIOInputsAutoLogged();
 
   public AdvancedSwerveTrajectoryFollower getFollower() {
     return follower;
@@ -155,7 +154,7 @@ private SwerveModule createModule(
   }
 
   /** Creates a new DrivebaseSubsystem. */
-  public DrivebaseSubsystem(VisionSubsystem visionSubsystem, DriveIO io) {
+  public DrivebaseSubsystem(VisionSubsystem visionSubsystem, DrivebaseSubsystemIO io) {
     this.visionSubsystem = visionSubsystem;
     this.io = io;
 

src/main/java/frc/robot/subsystems/DrivebaseSubsystemIO.java

@@ -0,0 +1,51 @@
+// Copyright 2021-2023 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface DrivebaseSubsystemIO {
+  @AutoLog
+  public static class DrivebaseSubsystemIOInputs {
+    public double frontLeftPositionRad = 0.0;
+    public double frontLeftVelocityRadPerSec = 0.0;
+    public double frontLeftAppliedVolts = 0.0;
+    public double[] frontLeftCurrentAmps = new double[] {};
+
+    public double backLeftPositionRad = 0.0;
+    public double backLeftVelocityRadPerSec = 0.0;
+    public double backLeftAppliedVolts = 0.0;
+    public double[] backLeftCurrentAmps = new double[] {};
+
+    public double frontRightPositionRad = 0.0;
+    public double frontRightVelocityRadPerSec = 0.0;
+    public double frontRightAppliedVolts = 0.0;
+    public double[] frontRightCurrentAmps = new double[] {};
+
+    public double backRightPositionRad = 0.0;
+    public double backRightVelocityRadPerSec = 0.0;
+    public double backRightAppliedVolts = 0.0;
+    public double[] backRightCurrentAmps = new double[] {};
+
+    public Rotation2d gyroYaw = new Rotation2d();
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(DrivebaseSubsystemIOInputs inputs) {}
+
+  /** Run open loop at the specified voltage. */
+  public default void setVoltage(
+      double frontLeftVolts, double backLeftVolts, double frontRightVolts, double backRightVolts) {}
+}

src/main/java/frc/robot/subsystems/ElevatorIO.java

@@ -12,18 +12,14 @@ public static class FlywheelIOInputs {
   }
 
   /** Updates the set of loggable inputs. */
-  public default void updateInputs(FlywheelIOInputs inputs) {
-  }
+  public default void updateInputs(FlywheelIOInputs inputs) {}
 
   /** Run closed loop at the specified velocity. */
-  public default void setVelocity(double velocityRadPerSec, double ffVolts) {
-  }
+  public default void setVelocity(double velocityRadPerSec, double ffVolts) {}
 
   /** Stop in open loop. */
-  public default void stop() {
-  }
+  public default void stop() {}
 
   /** Set velocity PID constants. */
-  public default void configurePID(double kP, double kI, double kD) {
-  }
+  public default void configurePID(double kP, double kI, double kD) {}
 }

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

@@ -266,7 +266,8 @@ private void percentDrivePeriodic() {
     if (wristFilterOutput > Elevator.STATOR_LIMIT) {
       wristMotor.set(TalonFXControlMode.PercentOutput, 0);
     } else {
-      wristMotor.set(TalonFXControlMode.PercentOutput, MathUtil.clamp(wristPercentControl, -0.75, 0.75));
+      wristMotor.set(
+          TalonFXControlMode.PercentOutput, MathUtil.clamp(wristPercentControl, -0.75, 0.75));
     }
   }