[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.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java

@@ -12,9 +12,9 @@ import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
@@ -39,18 +39,18 @@ public class RobotContainer {
   // The autonomous routines
 
   // A simple auto routine that drives forward a specified distance, and then stops.
-  private final Command m_simpleAuto = new StartEndCommand(
-      // Start driving forward at the start of the command
+  private final Command m_simpleAuto = new FunctionalCommand(
+      // Reset encoders on command start
+      m_robotDrive::resetEncoders,
+      // Drive forward while the command is executing
       () -> m_robotDrive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
       // Stop driving at the end of the command
-      () -> m_robotDrive.arcadeDrive(0, 0),
-      // Requires the drive subsystem
-      m_robotDrive)
-      // Reset the encoders before starting
-      .beforeStarting(m_robotDrive::resetEncoders)
+      interrupt -> m_robotDrive.arcadeDrive(0, 0),
       // End the command when the robot's driven distance exceeds the desired value
-      .withInterrupt(
-          () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches);
+      () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches,
+      // Require the drive subsystem
+      m_robotDrive
+  );
 
   // A complex auto routine that drives forward, drops a hatch, and then drives backward.
   private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-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,9 +7,9 @@
 
 package edu.wpi.first.wpilibj.examples.hatchbotinlined.commands;
 
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
 
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
@@ -28,28 +28,38 @@ public class ComplexAutoCommand extends SequentialCommandGroup {
   public ComplexAutoCommand(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
     addCommands(
         // Drive forward up to the front of the cargo ship
-        new StartEndCommand(
-            // Start driving forward at the start of the command
+        new FunctionalCommand(
+            // Reset encoders on command start
+            driveSubsystem::resetEncoders,
+            // Drive forward while the command is executing
             () -> driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
             // Stop driving at the end of the command
-            () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem)
-            // Reset the encoders before starting
-            .beforeStarting(driveSubsystem::resetEncoders)
+            interrupt -> driveSubsystem.arcadeDrive(0, 0),
             // End the command when the robot's driven distance exceeds the desired value
-            .withInterrupt(() -> driveSubsystem.getAverageEncoderDistance()
-                >= AutoConstants.kAutoDriveDistanceInches),
+            () -> driveSubsystem.getAverageEncoderDistance()
+                >= AutoConstants.kAutoDriveDistanceInches,
+            // Require the drive subsystem
+            driveSubsystem
+        ),
 
         // Release the hatch
         new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem),
 
         // Drive backward the specified distance
-        new StartEndCommand(
+        new FunctionalCommand(
+            // Reset encoders on command start
+            driveSubsystem::resetEncoders,
+            // Drive backward while the command is executing
             () -> driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0),
-            () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem)
-            .beforeStarting(driveSubsystem::resetEncoders)
-            .withInterrupt(
-                () -> driveSubsystem.getAverageEncoderDistance()
-                    <= -AutoConstants.kAutoBackupDistanceInches));
+            // Stop driving at the end of the command
+            interrupt -> driveSubsystem.arcadeDrive(0, 0),
+            // End the command when the robot's driven distance exceeds the desired value
+            () -> driveSubsystem.getAverageEncoderDistance()
+                <= AutoConstants.kAutoBackupDistanceInches,
+            // Require the drive subsystem
+            driveSubsystem
+        )
+    );
   }
 
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-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.                                                               */
@@ -27,6 +27,7 @@ public class DriveDistance extends CommandBase {
     m_distance = inches;
     m_speed = speed;
     m_drive = drive;
+    addRequirements(m_drive);
   }
 
   @Override
@@ -35,6 +36,11 @@ public class DriveDistance extends CommandBase {
     m_drive.arcadeDrive(m_speed, 0);
   }
 
+  @Override
+  public void execute() {
+    m_drive.arcadeDrive(m_speed, 0);
+  }
+
   @Override
   public void end(boolean interrupted) {
     m_drive.arcadeDrive(0, 0);