diff --git a/source/docs/software/commandbased/commands-v2/organizing-command-based.rst b/source/docs/software/commandbased/commands-v2/organizing-command-based.rst index 0111bb925f..a0d2937195 100644 --- a/source/docs/software/commandbased/commands-v2/organizing-command-based.rst +++ b/source/docs/software/commandbased/commands-v2/organizing-command-based.rst @@ -45,18 +45,18 @@ This is sufficient for commands that are only used once. However, for a command // RobotContainer.java intakeButton.whileTrue(Commands.startEnd(() -> intake.set(1.0), () -> intake.set(0.0), intake)); Command intakeAndShoot = Commands.startEnd(() -> intake.set(1.0), () -> intake.set(0.0), intake) - .alongWith(new RunShooter(shooter)); + .alongWith(new RunShooter(shooter)); Command autonomousCommand = Commands.sequence( - Commands.startEnd(() -> intake.set(1.0), () -> intake.set(0.0), intake).withTimeout(5.0), - Commands.waitSeconds(3.0), - Commands.startEnd(() -> intake.set(1.0), () -> intake.set(0.0), intake).withTimeout(5.0) + Commands.startEnd(() -> intake.set(1.0), () -> intake.set(0.0), intake).withTimeout(5.0), + Commands.waitSeconds(3.0), + Commands.startEnd(() -> intake.set(1.0), () -> intake.set(0.0), intake).withTimeout(5.0) ); ``` ```c++ intakeButton.WhileTrue(frc2::cmd::StartEnd([&intake] { intake.Set(1.0); }, [&intake] { intake.Set(0.0); }, {&intake})); frc2::CommandPtr intakeAndShoot = frc2::cmd::StartEnd([&intake] { intake.Set(1.0); }, [&intake] { intake.Set(0.0); }, {&intake}) - .AlongWith(RunShooter(&shooter).ToPtr()); + .AlongWith(RunShooter(&shooter).ToPtr()); frc2::CommandPtr autonomousCommand = frc2::cmd::Sequence( frc2::cmd::StartEnd([&intake] { intake.Set(1.0); }, [&intake] { intake.Set(0.0); }, {&intake}).WithTimeout(5.0_s), frc2::cmd::Wait(3.0_s), @@ -106,9 +106,9 @@ Using this new factory method in command groups and button bindings is highly ex intakeButton.whileTrue(intake.runIntakeCommand()); Command intakeAndShoot = intake.runIntakeCommand().alongWith(new RunShooter(shooter)); Command autonomousCommand = Commands.sequence( - intake.runIntakeCommand().withTimeout(5.0), - Commands.waitSeconds(3.0), - intake.runIntakeCommand().withTimeout(5.0) + intake.runIntakeCommand().withTimeout(5.0), + Commands.waitSeconds(3.0), + intake.runIntakeCommand().withTimeout(5.0) ); ``` @@ -128,7 +128,7 @@ Adding a parameter to the ``runIntakeCommand`` method to provide the exact perce ```java public Command runIntakeCommand(double percent) { - return new StartEndCommand(() -> this.set(percent), () -> this.set(0.0), this); + return new StartEndCommand(() -> this.set(percent), () -> this.set(0.0), this); } ``` @@ -145,15 +145,15 @@ For instance, this code creates a command group that runs the intake forwards fo ```java Command intakeRunSequence = intake.runIntakeCommand(1.0).withTimeout(2.0) - .andThen(Commands.waitSeconds(2.0)) - .andThen(intake.runIntakeCommand(-1.0).withTimeout(5.0)); + .andThen(Commands.waitSeconds(2.0)) + .andThen(intake.runIntakeCommand(-1.0).withTimeout(5.0)); ``` ```c++ frc2::CommandPtr intakeRunSequence = intake.RunIntakeCommand(1.0).WithTimeout(2.0_s) - .AndThen(frc2::cmd::Wait(2.0_s)) - .AndThen(intake.RunIntakeCommand(-1.0).WithTimeout(5.0_s)); - ``` + .AndThen(frc2::cmd::Wait(2.0_s)) + .AndThen(intake.RunIntakeCommand(-1.0).WithTimeout(5.0_s)); + ``` This approach is recommended for commands that are conceptually related to only a single subsystem, and is very concise. However, it doesn't fare well with commands related to more than one subsystem: passing in other subsystem objects is unintuitive and can cause race conditions and circular dependencies, and thus should be avoided. Therefore, this approach is best suited for single-subsystem commands, and should be used only for those cases. @@ -167,27 +167,39 @@ Instance factory methods work great for single-subsystem commands. However, com ```java public class AutoRoutines { - public static Command driveAndIntake(Drivetrain drivetrain, Intake intake) { - return Commands.sequence( - Commands.parallel( - drivetrain.driveCommand(0.5, 0.5), - intake.runIntakeCommand(1.0) - ).withTimeout(5.0), - Commands.parallel( - drivetrain.stopCommand(); - intake.stopCommand(); - ) - ); - } + public static Command driveAndIntake(Drivetrain drivetrain, Intake intake) { + return Commands.sequence( + Commands.parallel( + drivetrain.driveCommand(0.5, 0.5), + intake.runIntakeCommand(1.0) + ).withTimeout(5.0), + Commands.parallel( + drivetrain.stopCommand(); + intake.stopCommand(); + ) + ); + } } ``` ```c++ - // TODO + class AutoRoutines { + public: + static frc2::CommandPtr driveAndIntake(Drivetrain* drivetrain, Intake* intake) { + return frc2::cmd::Sequence( + frc2::cmd::Parallel( + drivetrain->driveCommand(0.5, 0.5), + intake->runIntakeCommand(1.0) + ).WithTimeout(5.0_s), + frc2::cmd::Parallel( + drivetrain->stopCommand(), + intake->stopCommand() + ) + ); + } + }; ``` -.. todo:: implement C++ version of the above code - #### Non-Static Command Factories If we want to avoid the verbosity of adding required subsystems as parameters to our factory methods, we can instead construct an instance of our ``AutoRoutines`` class and inject our subsystems through the constructor: @@ -195,41 +207,68 @@ If we want to avoid the verbosity of adding required subsystems as parameters to ```java public class AutoRoutines { - private Drivetrain drivetrain; - private Intake intake; - public AutoRoutines(Drivetrain drivetrain, Intake intake) { - this.drivetrain = drivetrain; - this.intake = intake; - } - public Command driveAndIntake() { - return Commands.sequence( - Commands.parallel( - drivetrain.driveCommand(0.5, 0.5), - intake.runIntakeCommand(1.0) - ).withTimeout(5.0), - Commands.parallel( - drivetrain.stopCommand(); - intake.stopCommand(); - ) - ); - } - public Command driveThenIntake() { - return Commands.sequence( - drivetrain.driveCommand(0.5, 0.5).withTimeout(5.0), - drivetrain.stopCommand(), - intake.runIntakeCommand(1.0).withTimeout(5.0), - intake.stopCommand() - ); - } + private Drivetrain drivetrain; + private Intake intake; + public AutoRoutines(Drivetrain drivetrain, Intake intake) { + this.drivetrain = drivetrain; + this.intake = intake; + } + public Command driveAndIntake() { + return Commands.sequence( + Commands.parallel( + drivetrain.driveCommand(0.5, 0.5), + intake.runIntakeCommand(1.0) + ).withTimeout(5.0), + Commands.parallel( + drivetrain.stopCommand(); + intake.stopCommand(); + ) + ); + } + public Command driveThenIntake() { + return Commands.sequence( + drivetrain.driveCommand(0.5, 0.5).withTimeout(5.0), + drivetrain.stopCommand(), + intake.runIntakeCommand(1.0).withTimeout(5.0), + intake.stopCommand() + ); + } } ``` ```c++ - // TODO + class AutoRoutines { + public: + AutoRoutines(Drivetrain* drivetrain, Intake* intake) { + m_drivetrain = drivetrain; + m_intake = intake; + } + frc2::CommandPtr driveAndIntake() { + return frc2::cmd::Sequence( + frc2::cmd::Parallel( + m_drivetrain->driveCommand(0.5, 0.5), + m_intake->runIntakeCommand(1.0) + ).WithTimeout(5.0_s), + frc2::cmd::Parallel( + m_drivetrain->stopCommand(), + m_intake->stopCommand() + ) + ); + } + frc2::CommandPtr driveThenIntake() { + return frc2::cmd::Sequence( + m_drivetrain->driveCommand(0.5, 0.5).WithTimeout(5.0_s), + m_drivetrain->stopCommand(), + m_intake->runIntakeCommand(1.0).WithTimeout(5.0_s), + m_intake->stopCommand() + ); + } + private: + Drivetrain* m_drivetrain; + Intake* m_intake; + }; ``` -.. todo:: implement C++ version of the above code - Then, elsewhere in our code, we can instantiate an single instance of this class and use it to produce several commands: .. tab-set-code:: @@ -245,11 +284,15 @@ Then, elsewhere in our code, we can instantiate an single instance of this class ``` ```c++ - // TODO + AutoRoutines autoRoutines = AutoRoutines(m_drivetrain, m_intake); + frc2::CommandPtr driveAndIntake = autoRoutines.driveAndIntake(); + frc2::CommandPtr driveThenIntake = autoRoutines.driveThenIntake(); + frc2::CommandPtr drivingAndIntakingSequence = frc2::cmd::Sequence( + autoRoutines.driveAndIntake(), + autoRoutines.driveThenIntake() + ); ``` -.. todo:: implement C++ version of the above code - #### Capturing State in Inline Commands Inline commands are extremely concise and expressive, but do not offer explicit support for commands that have their own internal state (such as a drivetrain trajectory following command, which may encapsulate an entire controller). This is often accomplished by instead writing a Command class, which will be covered later in this article. @@ -262,23 +305,31 @@ However, it is still possible to ergonomically write a stateful command composit ```java public Command turnToAngle(double targetDegrees) { - // Create a controller for the inline command to capture - PIDController controller = new PIDController(Constants.kTurnToAngleP, 0, 0); - // We can do whatever configuration we want on the created state before returning from the factory - controller.setPositionTolerance(Constants.kTurnToAngleTolerance); - // Try to turn at a rate proportional to the heading error until we're at the setpoint, then stop - return run(() -> arcadeDrive(0,-controller.calculate(gyro.getHeading(), targetDegrees))) - .until(controller::atSetpoint) - .andThen(runOnce(() -> arcadeDrive(0, 0))); + // Create a controller for the inline command to capture + PIDController controller = new PIDController(Constants.kTurnToAngleP, 0, 0); + // We can do whatever configuration we want on the created state before returning from the factory + controller.setPositionTolerance(Constants.kTurnToAngleTolerance); + // Try to turn at a rate proportional to the heading error until we're at the setpoint, then stop + return run(() -> arcadeDrive(0,-controller.calculate(gyro.getHeading(), targetDegrees))) + .until(controller::atSetpoint) + .andThen(runOnce(() -> arcadeDrive(0, 0))); } ``` ```c++ - // TODO + frc2::CommandPtr turnToAngle(double targetDegrees) { + // Create a controller for the inline command to capture + frc::PIDController controller{DrivetrainConstants::kTurnToAngleP, 0, 0}; + // We can do whatever configuration we want on the created state before returning from the factory + controller.SetTolerance(DrivetrainConstants::kTurnToAngleTolerance); + controller.SetSetpoint(targetDegrees); + // Try to turn at a rate proportional to the heading error until we're at the setpoint, then stop + return frc2::cmd::Run([this, controller] mutable {arcadeDrive(0,-controller.Calculate(gyro.getHeading()));}) + .Until([controller] {return controller.AtSetpoint();}) + .AndThen(frc2::cmd::RunOnce([this] {arcadeDrive(0, 0);})); + } ``` -.. todo:: implement C++ version of the above code - This pattern works very well in Java so long as the captured state is "effectively final" - i.e., it is never reassigned. This means that we cannot directly define and capture primitive types (e.g. `int`, `double`, `boolean`) - to circumvent this, we need to wrap any state primitives in a mutable container type (the same way `PIDController` wraps its internal `kP`, `kI`, and `kD` values). ### Writing Command Classes @@ -293,30 +344,44 @@ Returning to our simple intake command from earlier, we could do this by creatin ```java public class RunIntakeCommand extends Command { - private Intake m_intake; - public RunIntakeCommand(Intake intake) { - this.m_intake = intake; - addRequirements(intake); - } - @Override - public void initialize() { - m_intake.set(1.0); - } - @Override - public void end(boolean interrupted) { - m_intake.set(0.0); - } - // execute() defaults to do nothing - // isFinished() defaults to return false + private Intake m_intake; + public RunIntakeCommand(Intake intake) { + this.m_intake = intake; + addRequirements(intake); + } + @Override + public void initialize() { + m_intake.set(1.0); + } + @Override + public void end(boolean interrupted) { + m_intake.set(0.0); + } + // execute() defaults to do nothing + // isFinished() defaults to return false } ``` ```c++ - // TODO + class RunIntakeCommand : public frc2::CommandHelper { + public: + RunIntakeCommand(Intake* intake) { + m_intake = intake; + AddRequirements(m_intake); + } + void Initialize() override { + m_intake->set(1.0); + } + void End(bool interrupted) override { + m_intake->set(0.0); + } + // execute() defaults to do nothing + // isFinished() defaults to return false + private: + Intake* m_intake; + }; ``` -.. todo:: implement C++ version of the above code - This, however, is just as cumbersome as the original repetitive code, if not more verbose. The only two lines that really matter in this entire file are the two calls to ``intake.set()``, yet there are over 20 lines of boilerplate code! Not to mention, doing this for a lot of robot actions quickly clutters up a robot project with dozens of small files. Nevertheless, this might feel more "natural," particularly for programmers who prefer to stick closely to an object-oriented model. This approach should be used for commands with internal state (not subsystem state!), as the class can have fields to manage said state. It may also be more intuitive to write commands with complex logic as classes, especially for those less experienced with command composition. As the command is detached from any specific subsystem class and the required subsystem objects are injected through the constructor, this approach deals well with commands involving multiple subsystems. @@ -330,13 +395,13 @@ If we wish to write composite commands as their own classes, we may write a cons ```java public class IntakeThenOuttake extends SequentialCommandGroup { - public IntakeThenOuttake(Intake intake) { - super( - intake.runIntakeCommand(1.0).withTimeout(2.0), - new WaitCommand(2.0), - intake.runIntakeCommand(-1).withTimeout(5.0) - ); - } + public IntakeThenOuttake(Intake intake) { + super( + intake.runIntakeCommand(1.0).withTimeout(2.0), + new WaitCommand(2.0), + intake.runIntakeCommand(-1).withTimeout(5.0) + ); + } } ``` @@ -384,5 +449,4 @@ As with factory methods, state can be defined and captured within the command gr - Yes - Yes - Yes, but must obey capture rules - - Yes - + - Yes \ No newline at end of file