Command-Based Swerve Drive - Issue with progressing through commands

Technical Java
,
1

Hello.

We are running a command-based framework and having an issue with our autonomous program. It is designed to drive some distance with turns and then when it stops should run our conveyor and launcher systems.

Everything works except that when it finishes the last drive maneuver, the swerve drive wheels continue spinning at a slow speed moving the robot. This continues until the launcher command is finished.

Our hypothesis is that we are not ending the driving command correctly. We are looking for suggestions on how to troubleshoot this issue.

A link to our code is here (main): GitHub - YCSRobotics/SwerveBot-2024-Robot: 2024 wpilib robot code

2

Curious if we should be stringing our commands together with andThen

What we have now:

addCommands(
new InstantCommand(() → s_Swerve.setPose(exampleTrajectory.getInitialPose())),
swerveControllerCommand,
new AutonomousLauncherCmd(launcherSubsystem, 0.5, conveyorSubsystem, 1.0, 5)
);

Should we be doing this(?):

new InstantCommand(() → s_Swerve.setPose(exampleTrajectory.getInitialPose())),
swerveControllerCommand.andThen(() → new AutonomousLauncherCmd(launcherSubsystem, 0.5, conveyorSubsystem, 1.0, 5))

3

In the .andThen after the swerve command (or really just the next command), you can make an instant command that uses s_Swerve.drive and set all the arguments to 0. Your swerve looks a bit different from mine, but something like:

new InstantCommand(() -> s_Swerve.setPose(exampleTrajectory.getInitialPose())),
swerveControllerCommand,
new InstantCommand(() -> s_Swerve.drive(new Translation2d(0, 0), 0, false, false)),
new AutonomousLauncherCmd(launcherSubsystem, 0.5, conveyorSubsystem, 1.0, 5)
4

Thanks @Traptricker that’s a good idea.

I’m curious about when “andThen” should be used and when it is not needed. Do you have any experience with this?

5

command1.andThen(command2, …) returns a SequentialCommand.

There are two key types of commands for determining order of execution, although there are others, and I highly recommend you go through all of them to find the right commands to fit your needs: SequentialCommands and ParallelCommands.

A SequentialCommand runs the commands one after another, such that in my above example, command1 must finish before command2 will be scheduled by the CommandScheduler, and as many commands that need to run in “sequence” can be strung together as arguments to the method call.

A ParallelCommand, which can be created by calling command1.alongWith(command2, …), runs all the commands all at the same time, and finishes only once each and every command has finished.

You can combine the two concepts, for example, creating a parallel command of intaking continuously and a sequential command that drives a path and then shoots the game piece.

var autoCommand = (driveOnGeneratedPath.andThen(shoot)).alongWith(intakeContinuously);

Does that help?

Another useful method is .repeatedly() which returns a RepeatCommand, allowing you to run a command over and over and combine repeatedly with an .until() with a lambda expression returning a boolean (which becomes the get() method of Supplier), that way, once you want it to stop repeating, you can have logic to do so. From the example I gave, we might want to repeat the command (leaving out the logic for actually switching to a new path to follow), we could drive and pick up multiple notes by doing the following:

var fullAutoCommand = autoCommand.repeatedly().until(() → noMoreNotesToGrab)

where noMoreNotesToGrab is some piece of logic defined elsewhere to say your autonomous should be done and stop finding paths to the next note.

1 Like
6

Thank you. That is helpful and makes sense.

For our team’s code, it is then correct to use the “andThen” before starting the next sequential command as shown below?

new InstantCommand(() → s_Swerve.setPose(exampleTrajectory.getInitialPose())),
swerveControllerCommand.andThen(() → new AutonomousLauncherCmd(launcherSubsystem, 0.5, conveyorSubsystem, 1.0, 5))

Is the issue were are seeing caused by not sequencing through the sequential commands correctly because we do not have an “andThen” or because we need to explicitly tell the motors to stop as suggested above, or both?

7

It works out to being the same thing. A sequential command runs every command in it, one after another. .andThen runs a command after another command. I would say it is better practice to just do:

new InstantCommand(() → s_Swerve.setPose(exampleTrajectory.getInitialPose())),
swerveControllerCommand,
new AutonomousLauncherCmd(launcherSubsystem, 0.5, conveyorSubsystem, 1.0, 5)

inside the sequential command group, but having the .andThen() does the same thing.

8

If it gives you any idea how complex of Commands can be created using just a few different types of commands, here is a chunk of our code this year:

public static Command getDynamicAutonomous(AutoPositionList positions) {
    if (positions.isEmpty() || CommandSwerveDrivetrain.getInstance().isEmpty() || Turret.getInstance().isEmpty()
        || Intake.getInstance().isEmpty())
      return Commands.none();
    return CommandSwerveDrivetrain.getInstance().map((drivetrain) -> {
      return (Command) Commands.deadline(
          Commands.deferredProxy(
              () -> {
                timer.restart();
                return followPathToNextPositionCommand(drivetrain, positions)
                    .until(() -> timer.hasElapsed(0.25)
                        && !RobotContainer.getRobotState().isNoteInRobot()
                        && currentTargetAutoPosition.isSkippable()
                        && currentTargetAutoPosition.getType().equals(AutoPositionType.Shoot));
              })
              .alongWith(Commands.waitUntil(() -> !RobotContainer.getRobotState().isNoteInRobot()
                  || prevAutoPosition == null || prevAutoPosition.getType().equals(AutoPositionType.Shoot)))
              .repeatedly()
              .until(() -> positions.size() == 0 && currentTargetAutoPosition == null),
          // .andThen(Commands.waitSeconds(2.0)),
          RobotContainer.getShootCommand(
              () -> prevAutoPosition == null ? ShooterType.Speaker : prevAutoPosition.getShootOrStealNote()),
          RobotContainer.getIntakeAutonomouslyCommand());
    }).orElse(Commands.none());
  }

The reason I mention this is because as your commands get larger and you use the methods to string together commands and logic, be careful with parenthesis. It’s very easy to write code that looks correct, but parentheses are wrapped improperly, and because everything returns a command, they all have the same methods, so just be careful.

9

For the visual learners, I recommend this resource that explains command lifecycle methods, along with pretty pictures explaining how the groups work.

2 Likes
10

Ooooh, this is a great write up! Definitely going to use this.

11

Thanks everyone for the helpful suggestions and resources. Our team really appreciates that support :slight_smile:

12

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.