Currently, the CommandGroup class only allows one to add commands in parallel or in series. For parallel commands, all commands that are running parallel to each other start at the same time. However, I am seeking a solution to start a command, in parallel, when another command (in the same parallel group) is almost finished.
For example, a useful scenario would be to raise a robot’s elevator to full height as its decelerating near the end of a drive forward trajectory. This way, you won’t have the robot racing across the field at full speed with its elevator up, but still save time by having the elevator at the desired position when the robot’s drivetrain reaches its target.
To solve this issue, I created the QueueCommand class, which essentially starts a command once a conditional has been met. It’s designed to keep on running until the given inner command has terminated.
public class QueueCommand extends Command {
private Command command;
private QueueCommandConditional conditional;
private boolean executed;
public QueueCommand(Command command, QueueCommandConditional conditional) {
this.command = command;
this.conditional = conditional;
}
@Override
protected void initialize() {
this.executed = false;
}
@Override
protected void execute() {
if (this.conditional.isTrue() && !this.executed) {
this.command.start();
this.executed = true;
}
}
@Override
protected boolean isFinished() {
return this.executed && !this.command.isRunning();
}
@Override
protected void end() {
this.command.cancel();
}
@Override
protected void interrupted() {
this.end();
}
public interface QueueCommandConditional {
public boolean isTrue();
}
}
I’ve experienced some irregularities when using the QueueCommand inside a CommandGroup. For example:
CommandGroup auton = new CommandGroup();
DriveTrajectoryCommand drive = new DriveTrajectoryCommand(250);
auton.addSequential(CGUtils.parallel(
drive,
new QueueCommand(
new ElevatorPositionCommand("up"),
() -> drive.isAlmostFinished(100)
)
));
auton.addSequential(new IntakeCommand("Out"));
will function as expected. The robot will start driving forward, the elevator will rise when there are 100 ticks left in DriveTrajectoryCommand, and the IntakeCommand will start and stop when both DriveTrajectoryCommand and ElevatorPositionCommand has finished.
However, the following CommandGroup will produce an irregularity:
CommandGroup auton = new CommandGroup();
DriveTrajectoryCommand drive = new DriveTrajectoryCommand(250);
auton.addSequential(CGUtils.parallel(
drive,
new QueueCommand(
new ElevatorPositionCommand("up"),
() -> drive.isAlmostFinished(100)
)
));
auton.addSequential(new IntakeCommand("Out"));
auton.addSequential(CGUtils.parallel(
new ElevatorPositionCommand("down"),
new DriveTrajectoryCommand(50)
));
DriveTrajectoryCommand will start as normal, but will be interrupted when there are 100 ticks left. The next cycle (20 ms after DriveTrajectoryCommand has terminated), ElevatorPositionCommand(“up”) will initialize and complete. No other command after that will run. It seems like the second ElevatorPositionCommand(“down”) causes the initial DriveTrajectoryCommand to terminate when ElevatorPositionCommand(“up”) is initialized.
I’ve created a basic tester to help debug this issue without the roboRIO present:
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.*;
import java.util.Timer;
import java.util.TimerTask;
public class SubsystemTest {
static {
HLUsageReporting.SetImplementation(new HLUsageReporting.Interface() {
@Override
public void reportScheduler() {
}
@Override
public void reportPIDController(int num) {
}
@Override
public void reportSmartDashboard() {
}
});
RobotState.SetImplementation(new RobotStateInterface());
edu.wpi.first.wpilibj.Timer.SetImplementation(new edu.wpi.first.wpilibj.Timer.StaticInterface() {
@Override
public double getFPGATimestamp() {
return 0;
}
@Override
public double getMatchTime() {
return 0;
}
@Override
public void delay(double seconds) {
}
@Override
public edu.wpi.first.wpilibj.Timer.Interface newTimer() {
return null;
}
});
}
public static Drivetrain drivetrain = new Drivetrain();
public static Elevator elevator = new Elevator();
public static Intake intake = new Intake();
public static void main(String] args) throws Exception {
Timer timer = new Timer();
timer.schedule(new TimerTask() {
@Override
public void run() {
Scheduler.getInstance().run();
}
}, 0, 20);
CommandGroup auton = new CommandGroup();
DriveTrajectoryCommand drive = new DriveTrajectoryCommand(250);
auton.addSequential(CGUtils.parallel(
drive,
new QueueCommand(
new ElevatorPositionCommand("up"),
() -> drive.isAlmostFinished(100)
)
));
auton.addSequential(new IntakeCommand("Out"));
auton.addSequential(CGUtils.parallel(
new ElevatorPositionCommand("down")
// new DriveTrajectoryCommand(50)
));
auton.start();
}
public static class Drivetrain extends Subsystem {
@Override
protected void initDefaultCommand() {
}
}
public static class Elevator extends Subsystem {
@Override
protected void initDefaultCommand() {
}
}
public static class Intake extends Subsystem {
@Override
protected void initDefaultCommand() {
}
}
public static class TickCommand extends Command {
private int tick;
private int targetTick;
private String name;
public TickCommand(int targetTick) {
this(targetTick, "Base");
}
public TickCommand(int targetTick, String name) {
this.tick = 0;
this.targetTick = targetTick;
this.name = name;
}
@Override
protected void initialize() {
System.out.println(this.getClass().getSimpleName() + ":" + this.name + " initialized @ " + System.currentTimeMillis());
}
@Override
protected void end() {
System.out.println(this.getClass().getSimpleName() + ":" + this.name + " ended @ " + System.currentTimeMillis() + ", ticksLeft = " + (this.targetTick - this.tick));
}
@Override
protected void interrupted() {
System.out.println(this.getClass().getSimpleName() + ":" + this.name + " interrupted @ " + System.currentTimeMillis() + ", ticksLeft = " + (this.targetTick - this.tick));
}
@Override
protected void execute() {
this.tick++;
}
@Override
protected boolean isFinished() {
return this.tick >= this.targetTick;
}
public boolean isAlmostFinished(int ticksLeft) {
return this.tick + ticksLeft >= this.targetTick;
}
}
public static class DriveTrajectoryCommand extends TickCommand {
public DriveTrajectoryCommand(int ticks) {
super(ticks);
this.requires(SubsystemTest.drivetrain);
}
@Override
protected void interrupted() {
super.interrupted();
}
}
public static class ElevatorPositionCommand extends TickCommand {
public ElevatorPositionCommand(String name) {
super(75, name);
this.requires(SubsystemTest.elevator);
}
}
public static class IntakeCommand extends TickCommand {
public IntakeCommand(String name) {
super(1, name);
this.requires(SubsystemTest.intake);
}
}
public static class WaitCommand extends TickCommand {
public WaitCommand(int ticks) {
super(ticks);
}
}
}
CGUtils:
public class CGUtils {
public static CommandGroup sequential(Command... commands) {
CommandGroup commandGroup = new CommandGroup();
for (Command command : commands) {
commandGroup.addSequential(command);
}
return commandGroup;
}
public static CommandGroup parallel(Command... commands) {
CommandGroup commandGroup = new CommandGroup();
for (Command command : commands) {
commandGroup.addParallel(command);
}
return commandGroup;
}
}