[dmelcer9]

Last year, our team tried to have a modular autonomous command. The way this worked was something like this:

        autoDefenseChooser = new SendableChooser();
        autoSideChooser = new SendableChooser();
        ...
        
        autoDefenseChooser.addDefault("Do not drive forward", new WaitCommand(.01));
        autoDefenseChooser.addObject("Only move forward", new DriveStraight(.8,1));
        autoDefenseChooser.addObject("Go over terrain defense", new DriveStraight(1,2.5));
        autoDefenseChooser.addObject("Low bar", new DriveStraight(.8, 5));
       
        
        SmartDashboard.putData("Auto Defense", autoDefenseChooser);
        
        
        autoSideChooser.addDefault("Do not turn", new WaitCommand(.01));
        autoSideChooser.addObject("Turn Right", new MultiRotate(60,3));
        autoSideChooser.addObject("Turn Left", new MultiRotate(-60,3));
        
        SmartDashboard.putData("Auto Side Selector", autoSideChooser);

...

Then, in a CommandGroup created during AutonomousInit:

addSequential((Command)Robot.autoDefenseChooser.getSelected());

However, this runs into a few problems. The biggest problem was that the robot would crash if you enter Autonomous mode twice without power cycling the robot. This was because the same commands were reused, and if the same command is added to multiple command groups, the program throws an exception and crashes.

Here are two solutions to this: the boring way and the fun(ctional) way.

(Note that this post uses a lot of method references, so I recommend that you take a look at them beforehand if you aren't familiar with them).

1. The boring (but effective) way

First, create a command that does nothing. I'll call it DoNothingCommand. This command should immediately return true or be a timed command with a very short timeout.

For this, we will use a Supplier. This allows us to get a "fresh" instance of the same command every time (so that way we aren't adding the same instance of a command to different instances of a commandgroup).

In Robot.java:

	public static SendableChooser<Supplier<Command>> autoStep1 = new SendableChooser<>();
	public static SendableChooser<Supplier<Command>> autoStep2 = new SendableChooser<>();
        ...
        public void robotInit(){
                ...
		autoStep1.addDefault("Do nothing", DoNothingCommand::new);
		autoStep1.addObject("Go Straight",GoStraightCommand::new);
		autoStep1.addObject("Turn Left", TurnLeftCommand::new);
		autoStep1.addObject("Turn Right", TurnRightCommand::new);
		SmartDashboard.putData("Auto Step 1", autoStep1);
		
		
		autoStep2.addDefault("Do nothing", DoNothingCommand::new);
		autoStep2.addObject("Shoot Fuel", ShootFuelCommand::new);
		autoStep2.addObject("Place Gear", PlaceGearCommand::new);
		...
	}

Keep in mind that TurnRightCommand::new creates a Supplier<TurnRightCommand> that simply constructs a TurnRightCommand whenever the get method is called. For example:

		Supplier<TurnRightCommand> s = TurnRightCommand::new;
		TurnRightCommand c1 = s.get();
		TurnRightCommand c2 = s.get();
		System.out.println(c1 == c2);//false

Anyways, we can now construct a commandgroup from AutoInit. The constructor for the commandgroup can look something like this:

		addSequential(Robot.autoStep1.getSelected().get());
		addSequential(Robot.autoStep2.getSelected().get());

2. The fun way

This way throws an optional into the mix. An optional is meant to represent an object that may or may not be there, kind of like a null with superpowers.

In Robot.java:

        public static SendableChooser<Optional<Supplier<Command>>> autoStep1 = new SendableChooser<>();
	public static SendableChooser<Optional<Supplier<Command>>> autoStep2 = new SendableChooser<>();

         ...
         
         public void robotInit(){
                ...
                autoStep1.addDefault("Do nothing", Optional.empty());
		autoStep1.addObject("Go Straight", Optional.of(GoStraightCommand::new));
		autoStep1.addObject("Turn Left", Optional.of(TurnLeftCommand::new));
		autoStep1.addObject("Turn Right", Optional.of(TurnRightCommand::new));
		SmartDashboard.putData("Auto Step 1", autoStep1);
		
		
		autoStep2.addDefault("Do nothing", Optional.empty());
		autoStep2.addObject("Shoot Fuel", Optional.of(ShootFuelCommand::new));
		autoStep2.addObject("Place Gear", Optional.of(PlaceGearCommand::new));
                ...
        }

Note how all of the Suppliers are wrapped in Optionals. Now, instead of putting a DoNothingCommand, the "do nothing" option simply inserts an empty optional. In the command group itself, we can do something like this:

Robot.autoStep1.getSelected().map(Supplier::get).ifPresent(this::addSequential);
Robot.autoStep2.getSelected().map(Supplier::get).ifPresent(this::addSequential);

Let's break this down: the getSelected() method returns an Optional<Supplier<Command>>, meaning that it may contain an object that can create a new command. The map function says to take that Supplier<Command> and map it to the command. This function returns an Optional<Command>. Here's the trick- if the optional was empty in the first place, Java skips over the map step and simply returns another empty optional. Otherwise, it "Expands" the supplier and gets a fresh instance of a command from the supplier. If the optional contains a command, it will then add it to the command group.