RM_Loggable_Autochooser

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

@@ -14,12 +14,15 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.autochooser.FieldLocation;
 import frc.robot.constants.Constants;
 import frc.robot.utils.logging.commands.CommandLogger;
 
@@ -124,6 +127,13 @@ public void robotPeriodic() {
       SmartDashboard.putBoolean("Hub Active?", hubActive());
     }
 
+    // Puts data on the elastic dashboard
+    SmartDashboard.putString("Alliance Color", Robot.allianceColorString());
+    SmartDashboard.putBoolean("Hub Active?", hubActive());
+    SmartDashboard.putString("Selected Action", 
+      robotContainer.getAutoChooser().getCommandDescription());
+    SmartDashboard.putString("Starting Location", location().toString());
+
     // Gets the alliance color.
     if (DriverStation.isDSAttached() && allianceColor.isEmpty()) {
       allianceColor = DriverStation.getAlliance();
@@ -247,8 +257,11 @@ public static Diagnostics getDiagnostics() {
         return diagnostics;
     }
 
+  // Getters
   public boolean hubActive() {return hubActive;}
   public static Optional<Alliance> allianceColor() {return allianceColor;}
   public static String allianceColorString() {return String.valueOf(allianceColor.orElse(null));}
+  public FieldLocation location() {return robotContainer.getAutoChooser().getLocation();}
+  public Pose2d getStartingLocation() {return location().getLocation();}
 
-}
+}

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

@@ -20,12 +20,13 @@
 import frc.robot.commands.feeder.SpinFeeder;
 import frc.robot.commands.drive.FakeVision;
 import frc.robot.commands.intake.SpinIntake;
+import frc.robot.autochooser.AutoChooser;
 import frc.robot.commands.angler.AimAngler;
 import frc.robot.commands.shooter.SetShootingState;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.AnglerSubsystem;
-import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.ApriltagSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.FeederSubsystem;
 import frc.robot.constants.ShootingState;
 import frc.robot.constants.ShootingState.ShootState;
@@ -55,8 +56,9 @@
  * subsystems, commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
+    // Instantiate the autochooser.
+    private final AutoChooser autoChooser = new AutoChooser();
     // The robot's subsystems and commands are defined here...
-    //private final RollerSubsystem rollerSubsystem;
     //private final TiltSubsystem tiltSubsystem;
     private final AnglerSubsystem anglerSubsystem;
     private final IntakeSubsystem intakeSubsystem;
@@ -70,7 +72,7 @@ public class RobotContainer {
     private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT);
     private ShootingState shootState = new ShootingState(ShootState.STOPPED);
 
-    // Replace with CommandPS4Controller or CommandJoystick if needed
+        // Replace with CommandPS4Controller or CommandJoystick if needed
     //new CommandXboxController(OperatorConstants.kDriverControllerPort);private final CommandXboxController controller = new CommandXboxController(Constants.XBOX_CONTROLLER_PORT);
 
     /**
@@ -183,7 +185,7 @@ public void putShuffleboardCommands() {
                     "Tilt Up",
                     new TiltUp(tiltSubsystem));
 
-        SmartDashboard.putData(
+            SmartDashboard.putData(
                 "Tilt Down",
                 new TiltDown(tiltSubsystem));*/
 
@@ -250,8 +252,9 @@ public void putShuffleboardCommands() {
             SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem,new ApriltagReading(0, 0, 0, 0, 0, 0, 0)));
 
         }
+
     //basic drive command
-        if(!Constants.TESTBED){
+        if(!Constants.TESTBED) {
             Command driveDirectionTime = new DriveDirectionTime(drivebase, 0.1,0.1, true, 1);
             SmartDashboard.putData("Drive Command", driveDirectionTime);
             SmartDashboard.putData("Fake vision", new FakeVision(drivebase));
@@ -265,12 +268,15 @@ public void putShuffleboardCommands() {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        // An example command will be run in autonomous
-        return null;
+      return autoChooser.getCommand();
     }
 
     public RobotVisualizer getRobotVisualizer() {
-        return robotVisualizer;
+      return robotVisualizer;
+    }
+
+    public AutoChooser getAutoChooser() {
+      return autoChooser;
     }
 
     public IntakeSubsystem getIntakeSubsystem() {

src/main/java/frc/robot/autochooser/AutoAction.java

@@ -0,0 +1,37 @@
+package frc.robot.autochooser;
+
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.function.Function;
+import java.util.stream.Collectors;
+
+public enum AutoAction {
+  DO_NOTHING("Do Nothing"),
+  TWO_PIECE_HIGH("2 Piece L4"),
+  TWO_PIECE_LOW("2 Piece L2"),
+  ONE_PIECE("1 Piece"),
+  CROSS_THE_LINE("Cross The Line"),
+  INVALID("INVALID");
+  private final String name;
+  private static final HashMap<String, AutoAction> nameMap =
+      new HashMap<>(
+          Arrays.stream(AutoAction.values())
+              .collect(Collectors.toMap(AutoAction::getName, Function.identity())));
+
+  AutoAction(String name) {
+    this.name = name;
+  }
+
+  public String getName() {
+    return name;
+  }
+
+  @Override
+  public String toString() {
+    return getName();
+  }
+
+  public static AutoAction fromName(String name) {
+    return nameMap.get(name);
+  }
+}

src/main/java/frc/robot/autochooser/AutoChooser.java

@@ -0,0 +1,85 @@
+package frc.robot.autochooser;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class AutoChooser {
+
+    /** Drop-down chooser for the location. */
+    private LoggedDashboardChooser<FieldLocation> locationChooser;
+    /** Drop-down chooser for the action. */
+    private LoggedDashboardChooser<AutoAction> actionChooser;
+    /** Structure for mapping possible choices to commands. */
+    private final Map<AutoEvent, AutoCommand> commandMap = new HashMap<>();
+
+    private final AutoCommand DEFAULT_COMMAND = AutoCommand.Invalid;
+
+    public AutoChooser() {
+        this.locationChooser = new LoggedDashboardChooser<>(
+            "Location Chooser"
+        );
+        this.actionChooser = new LoggedDashboardChooser<>(
+            "Action Chooser"
+        );
+        populateChoosers();
+        populateMap();
+    }
+
+    /** Populates the drop-down choosers with enum constants. */
+    private void populateChoosers() {
+        for (FieldLocation location : FieldLocation.values()) {
+            switch (location) {
+                case INVALID -> {} // Skip the invalid case.
+                case ZERO -> { // Default
+                    locationChooser.addDefaultOption(location.toString(), location);
+                }
+                default -> {locationChooser.addOption(location.toString(), location);}
+            };
+        }
+        for (AutoAction action : AutoAction.values()) {
+            switch (action) {
+                case INVALID -> {} // Skip the invalid case.
+                case DO_NOTHING -> { // Default
+                    actionChooser.addDefaultOption(action.toString(), action);
+                }
+                default -> {actionChooser.addOption(action.toString(), action);}
+            };
+        }
+    }
+
+    /** Put mappings here.
+     *  @see AutoCommand */
+    private void populateMap() {
+        // Currently, we have some example mappings.
+        commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.LEFT),
+            AutoCommand.DoNothing);
+        commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.RIGHT), 
+            AutoCommand.DoSomething);
+    }
+
+    private AutoCommand get() {
+        AutoAction chosenAction = actionChooser.get();
+        FieldLocation chosenLocation = locationChooser.get();
+        AutoEvent event = new AutoEvent(chosenAction, chosenLocation);
+
+        return commandMap.getOrDefault(event, DEFAULT_COMMAND);
+    }
+
+    public LoggableCommand getCommand() {
+        return get().getCommand();
+    }
+
+    /** @return A human-readable description of the selected command. */
+    public String getCommandDescription() {
+        return get().getDescription();
+    }
+
+    public FieldLocation getLocation() {
+        return locationChooser.get();
+    }
+
+}

src/main/java/frc/robot/autochooser/AutoCommand.java

@@ -0,0 +1,47 @@
+package frc.robot.autochooser;
+
+import frc.robot.utils.logging.commands.LoggableCommand;
+import frc.robot.utils.logging.commands.DoNothingCommand;
+import frc.robot.utils.logging.commands.DoSomethingCommand;
+
+/** An enum to associate commands with human-readable descriptions. */
+public enum AutoCommand {
+
+    // Add commands here. Importantly, you should give each command
+    // a readable description so that the drive team can tell what
+    // the robot will actually do. This will be used to give the
+    // drive team visual feedback on the elastic dashboard when
+    // selecting an autonoumous command.
+    Invalid(
+        "The selection is invalid. (The robot won't do anything.)",
+        new DoNothingCommand()
+    ),
+    DoNothing("The robot won't do anything.", new DoNothingCommand()),
+    DoSomething(
+        "Something will be printed to the terminal.",
+        new DoSomethingCommand("""
+            SUCCESSFULLY
+
+            DID
+
+            SOMETHING
+            """)
+    );
+
+    private String description;
+    private LoggableCommand command;
+
+    AutoCommand(String description, LoggableCommand command) {
+        this.description = description;
+        this.command = command;
+    }
+
+    public String getDescription() {
+        return description;
+    }
+
+    public LoggableCommand getCommand() {
+        return command;
+    }
+
+}

src/main/java/frc/robot/autochooser/AutoEvent.java

@@ -0,0 +1,51 @@
+package frc.robot.autochooser;
+
+/**
+ * Wrapper Class, that Contains a {@link frc.robot.autochooser.AutoAction} and a {@link
+ * frc.robot.autochooser.FieldLocation}
+ */
+public class AutoEvent {
+  private final AutoAction action;
+  private final FieldLocation location;
+
+  public AutoEvent(AutoAction action, FieldLocation location) {
+    this.action = action;
+    this.location = location;
+  }
+
+  public AutoAction getAction() {
+    return action;
+  }
+
+  public FieldLocation getLocation() {
+    return location;
+  }
+
+  @Override
+  public int hashCode() {
+    final int prime = 31;
+    int result = 1;
+    result = prime * result + ((action == null) ? 0 : action.hashCode());
+    result = prime * result + ((location == null) ? 0 : location.hashCode());
+    return result;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (this == obj)
+      return true;
+    if (obj == null)
+      return false;
+    if (getClass() != obj.getClass())
+      return false;
+    AutoEvent other = (AutoEvent) obj;
+    if (action != other.action)
+      return false;
+    if (location != other.location)
+      return false;
+    return true;
+  }
+
+
+
+}