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