Skip to content

Command based c++ examples #3258

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Starelie wants to merge 2 commits into
base: 2027
Choose a base branch
from
Open

Command based c++ examples #3258

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1abfd48
Add c++ examples with references
Starelie Feb 27, 2026
5b59991
Add examples using pointers
Starelie Mar 3, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
262 changes: 163 additions & 99 deletions source/docs/software/commandbased/commands-v2/organizing-command-based.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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)
);
```

Expand All @@ -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);
}
```

Expand All @@ -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.

Expand All @@ -167,69 +167,108 @@ 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:

.. tab-set-code::

```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::
Expand All @@ -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.
Expand All @@ -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
Expand All @@ -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<frc2::Command, RunIntakeCommand> {
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.
Expand All @@ -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)
);
}
}
```

Expand Down Expand Up @@ -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
Loading