[wpilib] Fix bugs in Hatchbot examples (#2893)

This fixes an issue with some commands not correctly requiring their
subsytems. Furthermore, an execute() method was added to the
DriveDistance command to continuously update the voltage command.

wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,31 +7,47 @@
 
 #include "commands/ComplexAuto.h"
 
+#include <frc2/command/FunctionalCommand.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/ParallelRaceGroup.h>
-#include <frc2/command/StartEndCommand.h>
 
 using namespace AutoConstants;
 
 ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) {
   AddCommands(
       // Drive forward the specified distance
-      frc2::StartEndCommand([drive] { drive->ArcadeDrive(kAutoDriveSpeed, 0); },
-                            [drive] { drive->ArcadeDrive(0, 0); }, {drive})
-          .BeforeStarting([drive] { drive->ResetEncoders(); })
-          .WithInterrupt([drive] {
+      frc2::FunctionalCommand(
+          // Reset encoders on command start
+          [&] { drive->ResetEncoders(); },
+          // Drive forward while the command is executing
+          [&] { drive->ArcadeDrive(kAutoDriveSpeed, 0); },
+          // Stop driving at the end of the command
+          [&](bool interrupted) { drive->ArcadeDrive(0, 0); },
+          // End the command when the robot's driven distance exceeds the
+          // desired value
+          [&] {
             return drive->GetAverageEncoderDistance() >=
                    kAutoDriveDistanceInches;
-          }),
+          },
+          // Requires the drive subsystem
+          {drive}),
       // Release the hatch
       frc2::InstantCommand([hatch] { hatch->ReleaseHatch(); }, {hatch}),
       // Drive backward the specified distance
-      frc2::StartEndCommand(
-          [drive] { drive->ArcadeDrive(-kAutoDriveSpeed, 0); },
-          [drive] { drive->ArcadeDrive(0, 0); }, {drive})
-          .BeforeStarting([drive] { drive->ResetEncoders(); })
-          .WithInterrupt([drive] {
+      // Drive forward the specified distance
+      frc2::FunctionalCommand(
+          // Reset encoders on command start
+          [&] { drive->ResetEncoders(); },
+          // Drive backward while the command is executing
+          [&] { drive->ArcadeDrive(-kAutoDriveSpeed, 0); },
+          // Stop driving at the end of the command
+          [&](bool interrupted) { drive->ArcadeDrive(0, 0); },
+          // End the command when the robot's driven distance exceeds the
+          // desired value
+          [&] {
             return drive->GetAverageEncoderDistance() <=
                    kAutoBackupDistanceInches;
-          }));
+          },
+          // Requires the drive subsystem
+          {drive}));
 }

wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,11 +10,11 @@
 #include <frc/XboxController.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc2/command/Command.h>
+#include <frc2/command/FunctionalCommand.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/ParallelRaceGroup.h>
 #include <frc2/command/RunCommand.h>
 #include <frc2/command/SequentialCommandGroup.h>
-#include <frc2/command/StartEndCommand.h>
 
 #include "Constants.h"
 #include "commands/ComplexAuto.h"
@@ -47,15 +47,21 @@ class RobotContainer {
   HatchSubsystem m_hatch;
 
   // The autonomous routines
-  frc2::ParallelRaceGroup m_simpleAuto =
-      frc2::StartEndCommand(
-          [this] { m_drive.ArcadeDrive(ac::kAutoDriveSpeed, 0); },
-          [this] { m_drive.ArcadeDrive(0, 0); }, {&m_drive})
-          .BeforeStarting([this] { m_drive.ResetEncoders(); })
-          .WithInterrupt([this] {
-            return m_drive.GetAverageEncoderDistance() >=
-                   ac::kAutoDriveDistanceInches;
-          });
+  frc2::FunctionalCommand m_simpleAuto = frc2::FunctionalCommand(
+      // Reset encoders on command start
+      [this] { m_drive.ResetEncoders(); },
+      // Drive forward while the command is executing
+      [this] { m_drive.ArcadeDrive(AutoConstants::kAutoDriveSpeed, 0); },
+      // Stop driving at the end of the command
+      [this](bool interrupted) { m_drive.ArcadeDrive(0, 0); },
+      // End the command when the robot's driven distance exceeds the desired
+      // value
+      [this] {
+        return m_drive.GetAverageEncoderDistance() >=
+               AutoConstants::kAutoDriveDistanceInches;
+      },
+      // Requires the drive subsystem
+      {&m_drive});
   ComplexAuto m_complexAuto{&m_drive, &m_hatch};
 
   // Assorted commands to be bound to buttons

wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,6 +20,8 @@ void DriveDistance::Initialize() {
   m_drive->ArcadeDrive(m_speed, 0);
 }
 
+void DriveDistance::Execute() { m_drive->ArcadeDrive(m_speed, 0); }
+
 void DriveDistance::End(bool interrupted) { m_drive->ArcadeDrive(0, 0); }
 
 bool DriveDistance::IsFinished() {

wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -26,6 +26,8 @@ class DriveDistance
 
   void Initialize() override;
 
+  void Execute() override;
+
   void End(bool interrupted) override;
 
   bool IsFinished() override;