Commands that need current state (C++)

[tssmith7]

I have been having trouble with Commands that need to know the current state of the robot when they are triggered. This came about because we wanted to create a command that would determine which of the three stage climbing locations was closest and move to the correct position, climb, and insert the note in the trap. This was a fairly complex set of sequential commands. They way our team made this work was to subclass SequentialCommandGroup and then allocate an instance of it (with new) and schedule it each time the button was pressed. This worked.

I have recently been trying to move to factory methods for my commands and so I moved the complex sequential commands to a factory function returning a CommandPtr. I'm not sure the best way to get the same behavior as in the case where the command is a subclass of SequentialCommandGroup or if that is even possible.

The simplest case I could come up with is:

frc2::CommandPtr autos::StateCommand(ExampleSubsystem* subsystem ) {
  units::second_t timestart = frc::Timer::GetFPGATimestamp();
  
  return frc2::cmd::Sequence( 
    frc2::cmd::Wait( 1_s ),
    frc2::cmd::RunOnce([timestart] {
      fmt::print( "StateCommand got start time {} and end time of {}\n", timestart, frc::Timer::GetFPGATimestamp() );
    } )
  );
}

with the button binding being:

  joystick.Button(1).OnTrue( autos::StateCommand( &m_subsystem) ); 

When this is simply bound to a Trigger it prints the start time of when the code initialized and the correct ending time. Is there an elegant way to make this code work correctly? I can't use the same allocation trick since the factory method is not a class. Am I missing something?

The SequentialCommandGroup code that does work correctly is:

class StatefulCommand : public frc2::CommandHelper<frc2::SequentialCommandGroup, StatefulCommand> {
public:
    StatefulCommand( ExampleSubsystem *es ) { 
        units::second_t timestart = frc::Timer::GetFPGATimestamp();
        
        AddCommands( 
            frc2::WaitCommand( 1_s ),
            frc2::InstantCommand([timestart] {
            fmt::print( "StateCommand got start time {} and end time of {}\n", timestart, frc::Timer::GetFPGATimestamp() );
            }, {es} )
        );
    }
};

with the button binding code as:

   joystick.Button(2).OnTrue( frc2::cmd::RunOnce([this]
    { delete cptr;
      cptr = new StatefulCommand( &m_subsystem );
      cptr->Schedule(); 
    } )
  ); 

where cptr is a Command * initialized to nullptr as a member of the RobotContainer.

I could leave the commands that need state information as SequentialCommandGroup subclasses but I can't figure out how to add a CommandPtr type to the AddCommands() function for the other Commands that I have converted to factory methods. It is mentioned at the bottom of https://docs.wpilib.org/en/stable/docs/software/commandbased/cpp-command-discussion.html that Unwrap() is used for command compositions but I have not been able to figure out how to make that work.

[Starlight220]

DeferredCommand can always work for this stuff, just be careful to list requirements properly.

For the case of choosing between commands at schedule time, Select and Either exist.

[tssmith7]

This worked for a factory method that doesn't take parameters. I think can use Non Static Command Factories so I don't have to pass the subsystems as parameters. Thank you.

[tssmith7]

Actually I can't seem to get Non Static Command Factories to work. Do you have an example that uses DeferredCommand and uses subsystems?

[KangarooKoala]

Actually I can't seem to get Non Static Command Factories to work.

Could you elaborate more on what about non-static command factories isn't working?

Do you have an example that uses DeferredCommand and uses subsystems?

I haven't tested this, but this *should* work, or at least be on the right track:

joystick.Button(2).OnTrue(frc2::cmd::Defer([&m_subsystem] { return autos::StateCommand(&m_subsystem); }));

[tssmith7]

So at this point I ended up creating a base class that stores a std::unique_ptr<Command> object and fills it and schedules it whenever the command is needed. This is a working solution for me at this point.

The code I'm using is :

class ConstructOnDemandCommand {
public:
    ConstructOnDemandCommand() = default;
    
    virtual void Schedule() final { 
        cmd = CommandFactory().Unwrap();
        cmd->Schedule();
    } 
    
private:
    virtual frc2::CommandPtr CommandFactory() = 0;

    std::unique_ptr<frc2::Command> cmd;
};

This class is derived from and the CommandFactory() virtual function is overridden to provide the command.

If a class called TimerCmd derives from ConstructOnDemandCommand and RobotContainer has a member variable called timer of type TimerCmd then Trigger binding looks like :

joystick.Button(1).OnTrue( frc2::cmd::RunOnce([this] { timer.Schedule(); } ) ); 

This cleans up my ConfigureBindings() function and wraps up all the messy bits.