From 2056f0ce097ae1f6cc3d564fc05259dae8ddff74 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sat, 28 Nov 2020 17:01:56 -0500 Subject: [PATCH] [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. --- .../cpp/commands/ComplexAuto.cpp | 42 +++++++++++++------ .../HatchbotInlined/include/RobotContainer.h | 28 ++++++++----- .../cpp/commands/DriveDistance.cpp | 4 +- .../include/commands/DriveDistance.h | 4 +- .../hatchbotinlined/RobotContainer.java | 20 ++++----- .../commands/ComplexAutoCommand.java | 40 +++++++++++------- .../commands/DriveDistance.java | 8 +++- 7 files changed, 94 insertions(+), 52 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp index c5f928ba7b..ac8b6d9e97 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp +++ b/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 #include #include -#include 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})); } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h index 106812bd5c..89fa0d46ae 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h +++ b/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 #include #include +#include #include #include #include #include -#include #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 diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp index 6c7ef407e5..608186c12d 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp +++ b/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() { diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h index 6f350a9049..b2a9ef04d0 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java index d49fba45b1..e17bcb6714 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java +++ b/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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java index 45674da7e4..c8bde22500 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java +++ b/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 + ) + ); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java index d4abd71387..375b8799c4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java +++ b/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);