Adding a way to manage driver control profiles

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
 }
 
 java {

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

@@ -4,22 +4,28 @@
 
 package frc.robot;
 
-import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.geometry.Pose2d;
+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.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.constants.OIConstants;
 import frc.robot.interfaces.IDrivetrain;
 import frc.robot.subsystems.DifferentialDrivetrain;
+import frc.robot.subsystems.FrisbeeShooterWheels;
+import frc.robot.utilities.OIProfileManager;
 
 public class RobotContainer {
-  // TODO Not sure if the polymorphism will work correctly here when logging
-  @Logged(name = "Drivetrain")
   private IDrivetrain drivetrain;
+  private FrisbeeShooterWheels frisbeeShooterWheels;
 
   private CommandXboxController driver;
 
+  private OIProfileManager oiProfiles;
+
   public RobotContainer() {
     drivetrain = new DifferentialDrivetrain(
       new Pose2d(), 
@@ -27,12 +33,33 @@ public class RobotContainer {
       false
     );
 
+    frisbeeShooterWheels = new FrisbeeShooterWheels();
+
     driver = new CommandXboxController(OIConstants.kDriverUSB);
 
-    configureBindings();
+    oiProfiles = new OIProfileManager(
+      (SubsystemBase) drivetrain,
+      frisbeeShooterWheels
+    );
+
+    oiProfiles.addProfile("Brad's Profile", this::bradProfile);
+    oiProfiles.addProfile("Noah's Profile", this::noahProfile);
   }
 
-  private void configureBindings() {
+  public void configureShuffleboard() {
+    ShuffleboardTab operatorInterfaceTab = Shuffleboard.getTab(OIConstants.kOITabName);
+
+    operatorInterfaceTab.add("Profile Selector", oiProfiles)
+      .withPosition(0, 0)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kComboBoxChooser);
+  }
+
+  public Command getAutonomousCommand() {
+    return Commands.print("No autonomous command configured");
+  }
+
+  private void drivetrainConfig() {
     if (drivetrain instanceof DifferentialDrivetrain) {
       ((DifferentialDrivetrain) drivetrain).setDefaultCommand(
         drivetrain.drive(driver)
@@ -40,7 +67,27 @@ public class RobotContainer {
     }
   }
 
-  public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+  private void bradProfile() {
+    drivetrainConfig();
+
+    frisbeeShooterWheels.setDefaultCommand(
+      frisbeeShooterWheels.setSpeed(driver::getRightTriggerAxis)
+    );
+
+    driver.a().whileTrue(
+      frisbeeShooterWheels.setSpeed(() -> -.5)
+    );
+  }
+
+  private void noahProfile() {
+    drivetrainConfig();
+
+    frisbeeShooterWheels.setDefaultCommand(
+      frisbeeShooterWheels.setSpeed(driver::getLeftTriggerAxis)
+    );
+
+    driver.b().whileTrue(
+      frisbeeShooterWheels.setSpeed(() -> -.5)
+    );
   }
 }

src/main/java/frc/robot/constants/FrisbeeShooterWheelsConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class FrisbeeShooterWheelsConstants {
+    public static final int kFrontMotorCANID = 0;
+    public static final int kRearMotorCANID = 1;
+}

src/main/java/frc/robot/constants/OIConstants.java

@@ -2,4 +2,6 @@ package frc.robot.constants;
 
 public class OIConstants {
     public static final int kDriverUSB = 0;
+
+    public static final String kOITabName = "Operator Interface";
 }

src/main/java/frc/robot/interfaces/ISimpleMotor.java

@@ -1,9 +1,11 @@
 package frc.robot.interfaces;
 
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.wpilibj2.command.Command;
 
 public interface ISimpleMotor {
-    public Command setSpeed();
+    public Command setSpeed(DoubleSupplier speed);
 
     public Command stop();
 }

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

@@ -0,0 +1,32 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.FrisbeeShooterWheelsConstants;
+import frc.robot.interfaces.ISimpleMotor;
+
+public class FrisbeeShooterWheels extends SubsystemBase implements ISimpleMotor{
+    private SparkMax frontMotor;
+    private SparkMax rearMotor;
+
+    public FrisbeeShooterWheels() {
+        frontMotor = new SparkMax(FrisbeeShooterWheelsConstants.kFrontMotorCANID, MotorType.kBrushless);
+        rearMotor = new SparkMax(FrisbeeShooterWheelsConstants.kRearMotorCANID, MotorType.kBrushless);
+    }
+
+    public Command setSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            frontMotor.set(speed.getAsDouble());
+            rearMotor.set(speed.getAsDouble());
+        });
+    }
+
+    public Command stop() {
+        return setSpeed(() -> 0);
+    }
+}

src/main/java/frc/robot/utilities/OIProfileManager.java

@@ -0,0 +1,59 @@
+package frc.robot.utilities;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class OIProfileManager extends SendableChooser<Runnable> {
+    private SubsystemBase[] subsystems;
+
+    private Map<String, Runnable> profiles;
+
+    public OIProfileManager(SubsystemBase... subsystems) {
+        super();
+
+        this.subsystems = subsystems;
+
+        profiles = new HashMap<String, Runnable>();
+
+        this.onChange(this::onProfileChanged);
+    }
+
+    public void addProfile(String profileName, Runnable setupFunction) {
+        if(profiles.containsKey(profileName)) {
+            throw new IllegalArgumentException("Profile Name " + profileName + " already exists!");
+        }
+
+        profiles.put(profileName, setupFunction);
+        this.addOption(profileName, setupFunction);
+    }
+
+    // TODO This method has no way to reflect what it changed in the SendableChooser
+    public void switchProfile(String profileName) {
+        if(!profiles.containsKey(profileName)) {
+            throw new IllegalArgumentException("Profile Name " + profileName + " does not exist!");
+        }
+
+        clearCurrentConfiguration();
+
+        profiles.get(profileName).run();
+    }
+
+    private void clearCurrentConfiguration() {
+        CommandScheduler.getInstance().getActiveButtonLoop().clear();
+        
+        for(SubsystemBase s : subsystems) {
+            s.getCurrentCommand().cancel();
+            s.removeDefaultCommand();
+        }
+    }
+
+    private void onProfileChanged(Runnable toRun) {
+        clearCurrentConfiguration();
+
+        toRun.run();
+    }
+}