[commands] RepeatCommand: restart on following iteration (#4706)

This fixes InstantCommand.repeatedly().

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java

@@ -19,6 +19,7 @@ import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
  */
 public class RepeatCommand extends CommandBase {
   protected final Command m_command;
+  private boolean m_ended;
 
   /**
    * Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it
@@ -35,16 +36,21 @@ public class RepeatCommand extends CommandBase {
 
   @Override
   public void initialize() {
+    m_ended = false;
     m_command.initialize();
   }
 
   @Override
   public void execute() {
+    if (m_ended) {
+      m_ended = false;
+      m_command.initialize();
+    }
     m_command.execute();
     if (m_command.isFinished()) {
       // restart command
       m_command.end(false);
-      m_command.initialize();
+      m_ended = true;
     }
   }
 
@@ -62,4 +68,9 @@ public class RepeatCommand extends CommandBase {
   public boolean runsWhenDisabled() {
     return m_command.runsWhenDisabled();
   }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_command.getInterruptionBehavior();
+  }
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp

@@ -16,15 +16,20 @@ RepeatCommand::RepeatCommand(std::unique_ptr<Command>&& command) {
 }
 
 void RepeatCommand::Initialize() {
+  m_ended = false;
   m_command->Initialize();
 }
 
 void RepeatCommand::Execute() {
+  if (m_ended) {
+    m_ended = false;
+    m_command->Initialize();
+  }
   m_command->Execute();
   if (m_command->IsFinished()) {
     // restart command
     m_command->End(false);
-    m_command->Initialize();
+    m_ended = true;
   }
 }
 
@@ -39,3 +44,7 @@ void RepeatCommand::End(bool interrupted) {
 bool RepeatCommand::RunsWhenDisabled() const {
   return m_command->RunsWhenDisabled();
 }
+
+Command::InterruptionBehavior RepeatCommand::GetInterruptionBehavior() const {
+  return m_command->GetInterruptionBehavior();
+}

wpilibNewCommands/src/main/native/include/frc2/command/RepeatCommand.h

@@ -69,8 +69,11 @@ class RepeatCommand : public CommandHelper<CommandBase, RepeatCommand> {
 
   bool RunsWhenDisabled() const override;
 
+  Command::InterruptionBehavior GetInterruptionBehavior() const override;
+
  private:
   std::unique_ptr<Command> m_command;
+  bool m_ended;
 };
 }  // namespace frc2