001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj2.command; 006 007import static edu.wpi.first.units.Units.Second; 008import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; 009 010import edu.wpi.first.units.Measure; 011import edu.wpi.first.units.Time; 012import edu.wpi.first.util.function.BooleanConsumer; 013import edu.wpi.first.util.sendable.Sendable; 014import edu.wpi.first.util.sendable.SendableBuilder; 015import edu.wpi.first.util.sendable.SendableRegistry; 016import java.util.HashSet; 017import java.util.Set; 018import java.util.function.BooleanSupplier; 019 020/** 021 * A state machine representing a complete action to be performed by the robot. Commands are run by 022 * the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to build 023 * complicated multistep actions without the need to roll the state machine logic themselves. 024 * 025 * <p>Commands are run synchronously from the main robot loop; no multithreading is used, unless 026 * specified explicitly from the command implementation. 027 * 028 * <p>This class is provided by the NewCommands VendorDep 029 */ 030public abstract class Command implements Sendable { 031 /** Requirements set. */ 032 protected Set<Subsystem> m_requirements = new HashSet<>(); 033 034 /** Default constructor. */ 035 @SuppressWarnings("this-escape") 036 protected Command() { 037 String name = getClass().getName(); 038 SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1)); 039 } 040 041 /** The initial subroutine of a command. Called once when the command is initially scheduled. */ 042 public void initialize() {} 043 044 /** The main body of a command. Called repeatedly while the command is scheduled. */ 045 public void execute() {} 046 047 /** 048 * The action to take when the command ends. Called when either the command finishes normally, or 049 * when it interrupted/canceled. 050 * 051 * <p>Do not schedule commands here that share requirements with this command. Use {@link 052 * #andThen(Command...)} instead. 053 * 054 * @param interrupted whether the command was interrupted/canceled 055 */ 056 public void end(boolean interrupted) {} 057 058 /** 059 * Whether the command has finished. Once a command finishes, the scheduler will call its end() 060 * method and un-schedule it. 061 * 062 * @return whether the command has finished. 063 */ 064 public boolean isFinished() { 065 return false; 066 } 067 068 /** 069 * Specifies the set of subsystems used by this command. Two commands cannot use the same 070 * subsystem at the same time. If another command is scheduled that shares a requirement, {@link 071 * #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return 072 * an empty set. 073 * 074 * <p>Note: it is recommended that user implementations contain the requirements as a field, and 075 * return that field here, rather than allocating a new set every time this is called. 076 * 077 * @return the set of subsystems that are required 078 * @see InterruptionBehavior 079 */ 080 public Set<Subsystem> getRequirements() { 081 return m_requirements; 082 } 083 084 /** 085 * Adds the specified subsystems to the requirements of the command. The scheduler will prevent 086 * two commands that require the same subsystem from being scheduled simultaneously. 087 * 088 * <p>Note that the scheduler determines the requirements of a command when it is scheduled, so 089 * this method should normally be called from the command's constructor. 090 * 091 * @param requirements the requirements to add 092 */ 093 public final void addRequirements(Subsystem... requirements) { 094 for (Subsystem requirement : requirements) { 095 m_requirements.add(requireNonNullParam(requirement, "requirement", "addRequirements")); 096 } 097 } 098 099 /** 100 * Gets the name of this Command. 101 * 102 * <p>By default, the simple class name is used. This can be changed with {@link 103 * #setName(String)}. 104 * 105 * @return The display name of the Command 106 */ 107 public String getName() { 108 return SendableRegistry.getName(this); 109 } 110 111 /** 112 * Sets the name of this Command. 113 * 114 * @param name The display name of the Command. 115 */ 116 public void setName(String name) { 117 SendableRegistry.setName(this, name); 118 } 119 120 /** 121 * Gets the subsystem name of this Command. 122 * 123 * @return Subsystem name 124 */ 125 public String getSubsystem() { 126 return SendableRegistry.getSubsystem(this); 127 } 128 129 /** 130 * Sets the subsystem name of this Command. 131 * 132 * @param subsystem subsystem name 133 */ 134 public void setSubsystem(String subsystem) { 135 SendableRegistry.setSubsystem(this, subsystem); 136 } 137 138 /** 139 * Decorates this command with a timeout. If the specified timeout is exceeded before the command 140 * finishes normally, the command will be interrupted and un-scheduled. 141 * 142 * <p>Note: This decorator works by adding this command to a composition. The command the 143 * decorator was called on cannot be scheduled independently or be added to a different 144 * composition (namely, decorators), unless it is manually cleared from the list of composed 145 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 146 * returned from this method can be further decorated without issue. 147 * 148 * @param seconds the timeout duration 149 * @return the command with the timeout added 150 */ 151 public ParallelRaceGroup withTimeout(double seconds) { 152 return raceWith(new WaitCommand(seconds)); 153 } 154 155 /** 156 * Decorates this command with a timeout. If the specified timeout is exceeded before the command 157 * finishes normally, the command will be interrupted and un-scheduled. 158 * 159 * <p>Note: This decorator works by adding this command to a composition. The command the 160 * decorator was called on cannot be scheduled independently or be added to a different 161 * composition (namely, decorators), unless it is manually cleared from the list of composed 162 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 163 * returned from this method can be further decorated without issue. 164 * 165 * @param time the timeout duration 166 * @return the command with the timeout added 167 */ 168 public ParallelRaceGroup withTimeout(Measure<Time> time) { 169 return withTimeout(time.in(Second)); 170 } 171 172 /** 173 * Decorates this command with an interrupt condition. If the specified condition becomes true 174 * before the command finishes normally, the command will be interrupted and un-scheduled. 175 * 176 * <p>Note: This decorator works by adding this command to a composition. The command the 177 * decorator was called on cannot be scheduled independently or be added to a different 178 * composition (namely, decorators), unless it is manually cleared from the list of composed 179 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 180 * returned from this method can be further decorated without issue. 181 * 182 * @param condition the interrupt condition 183 * @return the command with the interrupt condition added 184 * @see #onlyWhile(BooleanSupplier) 185 */ 186 public ParallelRaceGroup until(BooleanSupplier condition) { 187 return raceWith(new WaitUntilCommand(condition)); 188 } 189 190 /** 191 * Decorates this command with a run condition. If the specified condition becomes false before 192 * the command finishes normally, the command will be interrupted and un-scheduled. 193 * 194 * <p>Note: This decorator works by adding this command to a composition. The command the 195 * decorator was called on cannot be scheduled independently or be added to a different 196 * composition (namely, decorators), unless it is manually cleared from the list of composed 197 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 198 * returned from this method can be further decorated without issue. 199 * 200 * @param condition the run condition 201 * @return the command with the run condition added 202 * @see #until(BooleanSupplier) 203 */ 204 public ParallelRaceGroup onlyWhile(BooleanSupplier condition) { 205 return until(() -> !condition.getAsBoolean()); 206 } 207 208 /** 209 * Decorates this command with a runnable to run before this command starts. 210 * 211 * <p>Note: This decorator works by adding this command to a composition. The command the 212 * decorator was called on cannot be scheduled independently or be added to a different 213 * composition (namely, decorators), unless it is manually cleared from the list of composed 214 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 215 * returned from this method can be further decorated without issue. 216 * 217 * @param toRun the Runnable to run 218 * @param requirements the required subsystems 219 * @return the decorated command 220 */ 221 public SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) { 222 return beforeStarting(new InstantCommand(toRun, requirements)); 223 } 224 225 /** 226 * Decorates this command with another command to run before this command starts. 227 * 228 * <p>Note: This decorator works by adding this command to a composition. The command the 229 * decorator was called on cannot be scheduled independently or be added to a different 230 * composition (namely, decorators), unless it is manually cleared from the list of composed 231 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 232 * returned from this method can be further decorated without issue. 233 * 234 * @param before the command to run before this one 235 * @return the decorated command 236 */ 237 public SequentialCommandGroup beforeStarting(Command before) { 238 return new SequentialCommandGroup(before, this); 239 } 240 241 /** 242 * Decorates this command with a runnable to run after the command finishes. 243 * 244 * <p>Note: This decorator works by adding this command to a composition. The command the 245 * decorator was called on cannot be scheduled independently or be added to a different 246 * composition (namely, decorators), unless it is manually cleared from the list of composed 247 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 248 * returned from this method can be further decorated without issue. 249 * 250 * @param toRun the Runnable to run 251 * @param requirements the required subsystems 252 * @return the decorated command 253 */ 254 public SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) { 255 return andThen(new InstantCommand(toRun, requirements)); 256 } 257 258 /** 259 * Decorates this command with a set of commands to run after it in sequence. Often more 260 * convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly. 261 * 262 * <p>Note: This decorator works by adding this command to a composition. The command the 263 * decorator was called on cannot be scheduled independently or be added to a different 264 * composition (namely, decorators), unless it is manually cleared from the list of composed 265 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 266 * returned from this method can be further decorated without issue. 267 * 268 * @param next the commands to run next 269 * @return the decorated command 270 */ 271 public SequentialCommandGroup andThen(Command... next) { 272 SequentialCommandGroup group = new SequentialCommandGroup(this); 273 group.addCommands(next); 274 return group; 275 } 276 277 /** 278 * Decorates this command with a set of commands to run parallel to it, ending when the calling 279 * command ends and interrupting all the others. Often more convenient/less-verbose than 280 * constructing a new {@link ParallelDeadlineGroup} explicitly. 281 * 282 * <p>Note: This decorator works by adding this command to a composition. The command the 283 * decorator was called on cannot be scheduled independently or be added to a different 284 * composition (namely, decorators), unless it is manually cleared from the list of composed 285 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 286 * returned from this method can be further decorated without issue. 287 * 288 * @param parallel the commands to run in parallel 289 * @return the decorated command 290 * @deprecated Use {@link deadlineFor} instead. 291 */ 292 @Deprecated(since = "2025", forRemoval = true) 293 public ParallelDeadlineGroup deadlineWith(Command... parallel) { 294 return new ParallelDeadlineGroup(this, parallel); 295 } 296 297 /** 298 * Decorates this command with a set of commands to run parallel to it, ending when the calling 299 * command ends and interrupting all the others. Often more convenient/less-verbose than 300 * constructing a new {@link ParallelDeadlineGroup} explicitly. 301 * 302 * <p>Note: This decorator works by adding this command to a composition. The command the 303 * decorator was called on cannot be scheduled independently or be added to a different 304 * composition (namely, decorators), unless it is manually cleared from the list of composed 305 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 306 * returned from this method can be further decorated without issue. 307 * 308 * @param parallel the commands to run in parallel. Note the parallel commands will be interupted 309 * when the deadline command ends 310 * @return the decorated command 311 */ 312 public ParallelDeadlineGroup deadlineFor(Command... parallel) { 313 return new ParallelDeadlineGroup(this, parallel); 314 } 315 316 /** 317 * Decorates this command with a set of commands to run parallel to it, ending when the last 318 * command ends. Often more convenient/less-verbose than constructing a new {@link 319 * ParallelCommandGroup} explicitly. 320 * 321 * <p>Note: This decorator works by adding this command to a composition. The command the 322 * decorator was called on cannot be scheduled independently or be added to a different 323 * composition (namely, decorators), unless it is manually cleared from the list of composed 324 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 325 * returned from this method can be further decorated without issue. 326 * 327 * @param parallel the commands to run in parallel 328 * @return the decorated command 329 */ 330 public ParallelCommandGroup alongWith(Command... parallel) { 331 ParallelCommandGroup group = new ParallelCommandGroup(this); 332 group.addCommands(parallel); 333 return group; 334 } 335 336 /** 337 * Decorates this command with a set of commands to run parallel to it, ending when the first 338 * command ends. Often more convenient/less-verbose than constructing a new {@link 339 * ParallelRaceGroup} explicitly. 340 * 341 * <p>Note: This decorator works by adding this command to a composition. The command the 342 * decorator was called on cannot be scheduled independently or be added to a different 343 * composition (namely, decorators), unless it is manually cleared from the list of composed 344 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 345 * returned from this method can be further decorated without issue. 346 * 347 * @param parallel the commands to run in parallel 348 * @return the decorated command 349 */ 350 public ParallelRaceGroup raceWith(Command... parallel) { 351 ParallelRaceGroup group = new ParallelRaceGroup(this); 352 group.addCommands(parallel); 353 return group; 354 } 355 356 /** 357 * Decorates this command to run repeatedly, restarting it when it ends, until this command is 358 * interrupted. The decorated command can still be canceled. 359 * 360 * <p>Note: This decorator works by adding this command to a composition. The command the 361 * decorator was called on cannot be scheduled independently or be added to a different 362 * composition (namely, decorators), unless it is manually cleared from the list of composed 363 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 364 * returned from this method can be further decorated without issue. 365 * 366 * @return the decorated command 367 */ 368 public RepeatCommand repeatedly() { 369 return new RepeatCommand(this); 370 } 371 372 /** 373 * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyCommand}. Use this for 374 * "forking off" from command compositions when the user does not wish to extend the command's 375 * requirements to the entire command composition. ProxyCommand has unique implications and 376 * semantics, see the WPILib docs for a full explanation. 377 * 378 * @return the decorated command 379 * @see ProxyCommand 380 * @see <a 381 * href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">WPILib 382 * docs</a> 383 */ 384 public ProxyCommand asProxy() { 385 return new ProxyCommand(this); 386 } 387 388 /** 389 * Decorates this command to only run if this condition is not met. If the command is already 390 * running and the condition changes to true, the command will not stop running. The requirements 391 * of this command will be kept for the new conditional command. 392 * 393 * <p>Note: This decorator works by adding this command to a composition. The command the 394 * decorator was called on cannot be scheduled independently or be added to a different 395 * composition (namely, decorators), unless it is manually cleared from the list of composed 396 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 397 * returned from this method can be further decorated without issue. 398 * 399 * @param condition the condition that will prevent the command from running 400 * @return the decorated command 401 * @see #onlyIf(BooleanSupplier) 402 */ 403 public ConditionalCommand unless(BooleanSupplier condition) { 404 return new ConditionalCommand(new InstantCommand(), this, condition); 405 } 406 407 /** 408 * Decorates this command to only run if this condition is met. If the command is already running 409 * and the condition changes to false, the command will not stop running. The requirements of this 410 * command will be kept for the new conditional command. 411 * 412 * <p>Note: This decorator works by adding this command to a composition. The command the 413 * decorator was called on cannot be scheduled independently or be added to a different 414 * composition (namely, decorators), unless it is manually cleared from the list of composed 415 * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition 416 * returned from this method can be further decorated without issue. 417 * 418 * @param condition the condition that will allow the command to run 419 * @return the decorated command 420 * @see #unless(BooleanSupplier) 421 */ 422 public ConditionalCommand onlyIf(BooleanSupplier condition) { 423 return unless(() -> !condition.getAsBoolean()); 424 } 425 426 /** 427 * Decorates this command to run or stop when disabled. 428 * 429 * @param doesRunWhenDisabled true to run when disabled. 430 * @return the decorated command 431 */ 432 public WrapperCommand ignoringDisable(boolean doesRunWhenDisabled) { 433 return new WrapperCommand(this) { 434 @Override 435 public boolean runsWhenDisabled() { 436 return doesRunWhenDisabled; 437 } 438 }; 439 } 440 441 /** 442 * Decorates this command to have a different {@link InterruptionBehavior interruption behavior}. 443 * 444 * @param interruptBehavior the desired interrupt behavior 445 * @return the decorated command 446 */ 447 public WrapperCommand withInterruptBehavior(InterruptionBehavior interruptBehavior) { 448 return new WrapperCommand(this) { 449 @Override 450 public InterruptionBehavior getInterruptionBehavior() { 451 return interruptBehavior; 452 } 453 }; 454 } 455 456 /** 457 * Decorates this command with a lambda to call on interrupt or end, following the command's 458 * inherent {@link #end(boolean)} method. 459 * 460 * @param end a lambda accepting a boolean parameter specifying whether the command was 461 * interrupted. 462 * @return the decorated command 463 */ 464 public WrapperCommand finallyDo(BooleanConsumer end) { 465 requireNonNullParam(end, "end", "Command.finallyDo()"); 466 return new WrapperCommand(this) { 467 @Override 468 public void end(boolean interrupted) { 469 super.end(interrupted); 470 end.accept(interrupted); 471 } 472 }; 473 } 474 475 /** 476 * Decorates this command with a lambda to call on interrupt or end, following the command's 477 * inherent {@link #end(boolean)} method. The provided lambda will run identically in both 478 * interrupt and end cases. 479 * 480 * @param end a lambda to run when the command ends, whether or not it was interrupted. 481 * @return the decorated command 482 */ 483 public WrapperCommand finallyDo(Runnable end) { 484 return finallyDo(interrupted -> end.run()); 485 } 486 487 /** 488 * Decorates this command with a lambda to call on interrupt, following the command's inherent 489 * {@link #end(boolean)} method. 490 * 491 * @param handler a lambda to run when the command is interrupted 492 * @return the decorated command 493 */ 494 public WrapperCommand handleInterrupt(Runnable handler) { 495 requireNonNullParam(handler, "handler", "Command.handleInterrupt()"); 496 return finallyDo( 497 interrupted -> { 498 if (interrupted) { 499 handler.run(); 500 } 501 }); 502 } 503 504 /** Schedules this command. */ 505 public void schedule() { 506 CommandScheduler.getInstance().schedule(this); 507 } 508 509 /** 510 * Cancels this command. Will call {@link #end(boolean) end(true)}. Commands will be canceled 511 * regardless of {@link InterruptionBehavior interruption behavior}. 512 * 513 * @see CommandScheduler#cancel(Command...) 514 */ 515 public void cancel() { 516 CommandScheduler.getInstance().cancel(this); 517 } 518 519 /** 520 * Whether the command is currently scheduled. Note that this does not detect whether the command 521 * is in a composition, only whether it is directly being run by the scheduler. 522 * 523 * @return Whether the command is scheduled. 524 */ 525 public boolean isScheduled() { 526 return CommandScheduler.getInstance().isScheduled(this); 527 } 528 529 /** 530 * Whether the command requires a given subsystem. 531 * 532 * @param requirement the subsystem to inquire about 533 * @return whether the subsystem is required 534 */ 535 public boolean hasRequirement(Subsystem requirement) { 536 return getRequirements().contains(requirement); 537 } 538 539 /** 540 * How the command behaves when another command with a shared requirement is scheduled. 541 * 542 * @return a variant of {@link InterruptionBehavior}, defaulting to {@link 543 * InterruptionBehavior#kCancelSelf kCancelSelf}. 544 */ 545 public InterruptionBehavior getInterruptionBehavior() { 546 return InterruptionBehavior.kCancelSelf; 547 } 548 549 /** 550 * Whether the given command should run when the robot is disabled. Override to return true if the 551 * command should run when disabled. 552 * 553 * @return whether the command should run when the robot is disabled 554 */ 555 public boolean runsWhenDisabled() { 556 return false; 557 } 558 559 /** 560 * Decorates this Command with a name. 561 * 562 * @param name name 563 * @return the decorated Command 564 */ 565 public WrapperCommand withName(String name) { 566 WrapperCommand wrapper = new WrapperCommand(Command.this) {}; 567 wrapper.setName(name); 568 return wrapper; 569 } 570 571 @Override 572 public void initSendable(SendableBuilder builder) { 573 builder.setSmartDashboardType("Command"); 574 builder.addStringProperty(".name", this::getName, null); 575 builder.addBooleanProperty( 576 "running", 577 this::isScheduled, 578 value -> { 579 if (value) { 580 if (!isScheduled()) { 581 schedule(); 582 } 583 } else { 584 if (isScheduled()) { 585 cancel(); 586 } 587 } 588 }); 589 builder.addBooleanProperty( 590 ".isParented", () -> CommandScheduler.getInstance().isComposed(this), null); 591 builder.addStringProperty( 592 "interruptBehavior", () -> getInterruptionBehavior().toString(), null); 593 builder.addBooleanProperty("runsWhenDisabled", this::runsWhenDisabled, null); 594 } 595 596 /** 597 * An enum describing the command's behavior when another command with a shared requirement is 598 * scheduled. 599 */ 600 public enum InterruptionBehavior { 601 /** 602 * This command ends, {@link #end(boolean) end(true)} is called, and the incoming command is 603 * scheduled normally. 604 * 605 * <p>This is the default behavior. 606 */ 607 kCancelSelf, 608 /** This command continues, and the incoming command is not scheduled. */ 609 kCancelIncoming 610 } 611}