Conditional Command Group

[barkinvar]

While looking at the 2910’s 2022 robot code, I noticed an interesting thing they did. They added commands to the Sequential Command Group on the initialize method to be able to change the command group with relation the some conditionals.

I guess this is easier than creating all possible combinations of command groups and running them on a conditional command. However, it seems like this implantation will cause some issues on the Scheduler side. As far as I understand at first this will prevent subsystem requirements to be defined since they are read before the initialize is called.

What other problems could this implementation cause and also what other methods can be used to create a similar code structure?

package org.frcteam2910.c2022.commands;

import edu.wpi.first.wpilibj2.command.*;
import org.frcteam2910.c2022.subsystems.ClimberSubsystem;
import org.frcteam2910.c2022.subsystems.ShooterSubsystem;
import org.frcteam2910.c2022.util.ClimbChooser;

public class AutoClimbCommand extends SequentialCommandGroup {
    private final ClimberSubsystem climber;
    private final ShooterSubsystem shooter;

    private final ClimbTypeSupplier climbTypeSupplier;

    private boolean hasAddedCommands;

    @FunctionalInterface
    public interface ClimbTypeSupplier {
        ClimbChooser.ClimbType getClimbType();
    }

    public AutoClimbCommand(ClimberSubsystem climber, ShooterSubsystem shooter, ClimbTypeSupplier climbTypeSupplier) {
        this.climber = climber;
        this.shooter = shooter;
        this.climbTypeSupplier = climbTypeSupplier;
    }

    @Override
    public void initialize() {
        if (!hasAddedCommands) {
            hasAddedCommands = true;
            ClimbChooser.ClimbType climbType = climbTypeSupplier.getClimbType();
            // Climber is currently hooked on the mid rung, but the robot is on the ground
            addCommands(new InstantCommand(shooter::disableFlywheel));

            // Prepare to transfer mid rung to hood
            addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_TRANSFER_HEIGHT, true, false)
                    .alongWith(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE)));

            // Transfer mid rung to hood
            addCommands(transferToHood(climber, shooter));
            if (climbType != ClimbChooser.ClimbType.MID) {
                // Move from mid rung to high rung
                if (climbType == ClimbChooser.ClimbType.HIGH_PARTWAY) {
                    addCommands(traverseToNextRung(climber, shooter, false, true));
                    addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_MIN_ANGLE));
                } else {
                    addCommands(traverseToNextRung(climber, shooter));
                }
                if (climbType != ClimbChooser.ClimbType.HIGH_PARTWAY) {
                    // Transfer high rung to hood
                    addCommands(transferToHood(climber, shooter));
                    if (climbType != ClimbChooser.ClimbType.HIGH_HOOK) {
                        // Move from high rung to traverse rung
                        if (climbType == ClimbChooser.ClimbType.TRAVERSAL_PARTWAY) {
                            addCommands(traverseToNextRung(climber, shooter, true, true));
                        } else {
                            addCommands(traverseToNextRung(climber, shooter, true, false));
                        }
                        if (climbType != ClimbChooser.ClimbType.TRAVERSAL_PARTWAY) {
                            addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_PASSAGE_HEIGHT, false,
                                    false));
                            // Transfer traverse rung to hood
                            addCommands(transferToHood(climber, shooter));
                        }
                    }
                }
            }
            addCommands(new InstantCommand(() -> climber.setTargetVoltage(0.0)));
            addCommands(new WaitCommand(5).perpetually());
            super.initialize();
        }
    }

    private static Command transferToHood(ClimberSubsystem climber, ShooterSubsystem shooter) {
        SequentialCommandGroup group = new SequentialCommandGroup();

        // Move the climber to the transfer height
        group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_TRANSFER_HEIGHT));
        // Grab the rung with the hood
        group.addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_TRANSFER_ANGLE));
        // Release the rung with the climber so the hood can move freely
        group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_PASSAGE_HEIGHT));

        return group;
    }

    private static Command traverseToNextRung(ClimberSubsystem climber, ShooterSubsystem shooter) {
        return traverseToNextRung(climber, shooter, false, false);
    }

    private static Command traverseToNextRung(ClimberSubsystem climber, ShooterSubsystem shooter, boolean traversal,
            boolean partway) {
        SequentialCommandGroup group = new SequentialCommandGroup();

        if (!traversal) {
            group.addCommands(
                    new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_TRAVERSE_EXTEND_ANGLE_HIGH, true, true)
                            .alongWith(new ClimberToPointCommand(climber, ClimberSubsystem.TRAVERSE_EXTEND_HEIGHT)));
        } else {
            // Angle the robot to extend the climber
            group.addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_TRAVERSE_EXTEND_ANGLE_TRAVERSE,
                    false, true));
            // Extend the climber slightly past the next rung
            group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.TRAVERSE_EXTEND_HEIGHT));
        }
        // Angle the robot so the climber hooks will grab the next rung
        group.addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_TRAVERSE_RETRACT_ANGLE, false, true));

        if (traversal) {
            if (partway) {
                group.addCommands(
                        new ClimberToPointCommand(climber, ClimberSubsystem.TRAVERSE_RUNG_PARTWAY_HEIGHT, false, false)
                                .alongWith(new SetHoodAngleCommand(shooter,
                                        ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE, true, true)));
                group.addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_MIN_ANGLE));
            } else {
                group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_PASSAGE_HEIGHT, false, false)
                        .alongWith(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE, true,
                                true)));
            }
        } else {
            // Retract the climber, and move the hood to the transfer position after the
            // climber grabs onto the next rung
            if (!partway) {
                group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_PASSAGE_HEIGHT, false, false)
                        .alongWith(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE, true,
                                true)));
            } else {
                group.addCommands(
                        new ClimberToPointCommand(climber, ClimberSubsystem.TRAVERSE_RUNG_PARTWAY_HEIGHT, false, false)
                                .alongWith(new SetHoodAngleCommand(shooter,
                                        ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE, true, true)));
            }
        }

        return group;
    }
}

[Waz]

When I first looked at the code you posted, I was a bit surprised that the addCommands calls in initialize were not throwing exceptions about adding commands to a running sequence. So, I took a look at the addCommands implementation in SequentialCommandGroup and saw that it could work.

Take a look at SequentialCommandGroup.addCommands and bring up a call hierarchy on m_currentCommandIndex. In particular, take a look at its initialization, and its usages in addCommands and initialize. The addCommands method will not start throwing exceptions until after SequentialCommandGroup.initialize is called which the posted code does not call until after adding commands.

One thing I would definitly change in this code would be to move super.initialize outside the if block so that it would be called everytime and not just the first time. They have gotten away with this since an auto routine is typically only scheduled once per match. Making this change could make repeated testing in your lab easier.

I have to wonder if the wpilib team really intended for this sort of thing to work. It seems to be a rather dangerous thing to do. Think of a teleop command that will get initialized multiple times during a match. The posted code would only add the commands once (but have initialization issues for all but the first time without my suggested change). I could see less experienced programmers getting into serious trouble with this pattern.

[Ryan_Blue]

I’m not sure if this was intended to work, but my guess is no, at least not in this manner.
There is no way to remove commands from a group. 2910 worked around this by checking to see if commands had already been added, and only then checking the condition and adding the appropriate commands. Unfortunately, this means the condition is only checked on the first schedule of the command, and after that you’re stuck with it.

ProxyCommand, SelectCommand and ConditionalCommand exist for this purpose- in this particular instance, ProxyCommand could be used with the Supplier<Command> constructor.