diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index 83c97216ba..248368de92 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.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. */ @@ -51,6 +51,13 @@ ParallelRaceGroup Command::WithInterrupt(std::function condition) && { SequentialCommandGroup Command::BeforeStarting( std::function toRun, std::initializer_list requirements) && { + return std::move(*this).BeforeStarting( + std::move(toRun), + wpi::makeArrayRef(requirements.begin(), requirements.end())); +} + +SequentialCommandGroup Command::BeforeStarting( + std::function toRun, wpi::ArrayRef requirements) && { std::vector> temp; temp.emplace_back( std::make_unique(std::move(toRun), requirements)); @@ -61,6 +68,13 @@ SequentialCommandGroup Command::BeforeStarting( SequentialCommandGroup Command::AndThen( std::function toRun, std::initializer_list requirements) && { + return std::move(*this).AndThen( + std::move(toRun), + wpi::makeArrayRef(requirements.begin(), requirements.end())); +} + +SequentialCommandGroup Command::AndThen( + std::function toRun, wpi::ArrayRef requirements) && { std::vector> temp; temp.emplace_back(std::move(*this).TransferOwnership()); temp.emplace_back( diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp index 323ed6776e..f2cfa956ab 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.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. */ @@ -21,6 +21,10 @@ void CommandBase::AddRequirements( m_requirements.insert(requirements.begin(), requirements.end()); } +void CommandBase::AddRequirements(wpi::ArrayRef requirements) { + m_requirements.insert(requirements.begin(), requirements.end()); +} + void CommandBase::AddRequirements(wpi::SmallSet requirements) { m_requirements.insert(requirements.begin(), requirements.end()); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 1f97ea239e..842facbdbf 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.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. */ @@ -248,6 +248,12 @@ void CommandScheduler::RegisterSubsystem( } } +void CommandScheduler::RegisterSubsystem(wpi::ArrayRef subsystems) { + for (auto* subsystem : subsystems) { + RegisterSubsystem(subsystem); + } +} + void CommandScheduler::UnregisterSubsystem( std::initializer_list subsystems) { for (auto* subsystem : subsystems) { @@ -255,6 +261,13 @@ void CommandScheduler::UnregisterSubsystem( } } +void CommandScheduler::UnregisterSubsystem( + wpi::ArrayRef subsystems) { + for (auto* subsystem : subsystems) { + UnregisterSubsystem(subsystem); + } +} + Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const { auto&& find = m_impl->subsystems.find(subsystem); if (find != m_impl->subsystems.end()) { diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp index ef7c40a22c..ee28ba79ab 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.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,18 @@ FunctionalCommand::FunctionalCommand( AddRequirements(requirements); } +FunctionalCommand::FunctionalCommand(std::function onInit, + std::function onExecute, + std::function onEnd, + std::function isFinished, + wpi::ArrayRef requirements) + : m_onInit{std::move(onInit)}, + m_onExecute{std::move(onExecute)}, + m_onEnd{std::move(onEnd)}, + m_isFinished{std::move(isFinished)} { + AddRequirements(requirements); +} + void FunctionalCommand::Initialize() { m_onInit(); } void FunctionalCommand::Execute() { m_onExecute(); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp index b199074780..6f66c5cf35 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.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. */ @@ -15,6 +15,12 @@ InstantCommand::InstantCommand(std::function toRun, AddRequirements(requirements); } +InstantCommand::InstantCommand(std::function toRun, + wpi::ArrayRef requirements) + : m_toRun{std::move(toRun)} { + AddRequirements(requirements); +} + InstantCommand::InstantCommand() : m_toRun{[] {}} {} void InstantCommand::Initialize() { m_toRun(); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index 15fb254fbe..88098037e7 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.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. */ @@ -50,6 +50,46 @@ MecanumControllerCommand::MecanumControllerCommand( AddRequirements(requirements); } +MecanumControllerCommand::MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SimpleMotorFeedforward feedforward, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function currentWheelSpeeds, + frc2::PIDController frontLeftController, + frc2::PIDController rearLeftController, + frc2::PIDController frontRightController, + frc2::PIDController rearRightController, + std::function + output, + wpi::ArrayRef requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_feedforward(feedforward), + m_kinematics(kinematics), + m_xController(std::make_unique(xController)), + m_yController(std::make_unique(yController)), + m_thetaController( + std::make_unique>( + thetaController)), + m_maxWheelVelocity(maxWheelVelocity), + m_frontLeftController( + std::make_unique(frontLeftController)), + m_rearLeftController( + std::make_unique(rearLeftController)), + m_frontRightController( + std::make_unique(frontRightController)), + m_rearRightController( + std::make_unique(rearRightController)), + m_currentWheelSpeeds(currentWheelSpeeds), + m_outputVolts(output), + m_usePID(true) { + AddRequirements(requirements); +} + MecanumControllerCommand::MecanumControllerCommand( frc::Trajectory trajectory, std::function pose, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, @@ -74,6 +114,30 @@ MecanumControllerCommand::MecanumControllerCommand( AddRequirements(requirements); } +MecanumControllerCommand::MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function + output, + wpi::ArrayRef requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_kinematics(kinematics), + m_xController(std::make_unique(xController)), + m_yController(std::make_unique(yController)), + m_thetaController( + std::make_unique>( + thetaController)), + m_maxWheelVelocity(maxWheelVelocity), + m_outputVel(output), + m_usePID(false) { + AddRequirements(requirements); +} + void MecanumControllerCommand::Initialize() { m_prevTime = 0_s; auto initialState = m_trajectory.Sample(0_s); diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp index c4eac81265..1b13d75884 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.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. */ @@ -16,6 +16,13 @@ NotifierCommand::NotifierCommand(std::function toRun, AddRequirements(requirements); } +NotifierCommand::NotifierCommand(std::function toRun, + units::second_t period, + wpi::ArrayRef requirements) + : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} { + AddRequirements(requirements); +} + NotifierCommand::NotifierCommand(NotifierCommand&& other) : CommandHelper(std::move(other)), m_toRun(other.m_toRun), diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp index 4a956f6106..8ce3c3785b 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.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. */ @@ -21,6 +21,18 @@ PIDCommand::PIDCommand(PIDController controller, AddRequirements(requirements); } +PIDCommand::PIDCommand(PIDController controller, + std::function measurementSource, + std::function setpointSource, + std::function useOutput, + wpi::ArrayRef requirements) + : m_controller{controller}, + m_measurement{std::move(measurementSource)}, + m_setpoint{std::move(setpointSource)}, + m_useOutput{std::move(useOutput)} { + AddRequirements(requirements); +} + PIDCommand::PIDCommand(PIDController controller, std::function measurementSource, double setpoint, std::function useOutput, @@ -28,6 +40,13 @@ PIDCommand::PIDCommand(PIDController controller, : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; }, useOutput, requirements) {} +PIDCommand::PIDCommand(PIDController controller, + std::function measurementSource, + double setpoint, std::function useOutput, + wpi::ArrayRef requirements) + : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; }, + useOutput, requirements) {} + void PIDCommand::Initialize() { m_controller.Reset(); } void PIDCommand::Execute() { diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index 261896f274..b1608bfe69 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.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. */ @@ -32,6 +32,28 @@ RamseteCommand::RamseteCommand( AddRequirements(requirements); } +RamseteCommand::RamseteCommand( + frc::Trajectory trajectory, std::function pose, + frc::RamseteController controller, + frc::SimpleMotorFeedforward feedforward, + frc::DifferentialDriveKinematics kinematics, + std::function wheelSpeeds, + frc2::PIDController leftController, frc2::PIDController rightController, + std::function output, + wpi::ArrayRef requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_controller(controller), + m_feedforward(feedforward), + m_kinematics(kinematics), + m_speeds(wheelSpeeds), + m_leftController(std::make_unique(leftController)), + m_rightController(std::make_unique(rightController)), + m_outputVolts(output), + m_usePID(true) { + AddRequirements(requirements); +} + RamseteCommand::RamseteCommand( frc::Trajectory trajectory, std::function pose, frc::RamseteController controller, @@ -48,6 +70,22 @@ RamseteCommand::RamseteCommand( AddRequirements(requirements); } +RamseteCommand::RamseteCommand( + frc::Trajectory trajectory, std::function pose, + frc::RamseteController controller, + frc::DifferentialDriveKinematics kinematics, + std::function + output, + wpi::ArrayRef requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_controller(controller), + m_kinematics(kinematics), + m_outputVel(output), + m_usePID(false) { + AddRequirements(requirements); +} + void RamseteCommand::Initialize() { m_prevTime = 0_s; auto initialState = m_trajectory.Sample(0_s); diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp index 5c2c75534d..dff0ffeb29 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.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. */ @@ -15,4 +15,10 @@ RunCommand::RunCommand(std::function toRun, AddRequirements(requirements); } +RunCommand::RunCommand(std::function toRun, + wpi::ArrayRef requirements) + : m_toRun{std::move(toRun)} { + AddRequirements(requirements); +} + void RunCommand::Execute() { m_toRun(); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp index 33407bf718..ad54ae0cb0 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.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. */ @@ -16,6 +16,13 @@ StartEndCommand::StartEndCommand(std::function onInit, AddRequirements(requirements); } +StartEndCommand::StartEndCommand(std::function onInit, + std::function onEnd, + wpi::ArrayRef requirements) + : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} { + AddRequirements(requirements); +} + StartEndCommand::StartEndCommand(const StartEndCommand& other) : CommandHelper(other) { m_onInit = other.m_onInit; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp index 440675f061..d8a7372313 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.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. */ @@ -22,6 +22,12 @@ Button Button::WhenPressed(std::function toRun, return *this; } +Button Button::WhenPressed(std::function toRun, + wpi::ArrayRef requirements) { + WhenActive(std::move(toRun), requirements); + return *this; +} + Button Button::WhileHeld(Command* command, bool interruptible) { WhileActiveContinous(command, interruptible); return *this; @@ -33,6 +39,12 @@ Button Button::WhileHeld(std::function toRun, return *this; } +Button Button::WhileHeld(std::function toRun, + wpi::ArrayRef requirements) { + WhileActiveContinous(std::move(toRun), requirements); + return *this; +} + Button Button::WhenHeld(Command* command, bool interruptible) { WhileActiveOnce(command, interruptible); return *this; @@ -49,6 +61,12 @@ Button Button::WhenReleased(std::function toRun, return *this; } +Button Button::WhenReleased(std::function toRun, + wpi::ArrayRef requirements) { + WhenInactive(std::move(toRun), requirements); + return *this; +} + Button Button::ToggleWhenPressed(Command* command, bool interruptible) { ToggleWhenActive(command, interruptible); return *this; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp index caf188ee0b..5c1a8f707b 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.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. */ @@ -30,6 +30,12 @@ Trigger Trigger::WhenActive(Command* command, bool interruptible) { Trigger Trigger::WhenActive(std::function toRun, std::initializer_list requirements) { + return WhenActive(std::move(toRun), wpi::makeArrayRef(requirements.begin(), + requirements.end())); +} + +Trigger Trigger::WhenActive(std::function toRun, + wpi::ArrayRef requirements) { return WhenActive(InstantCommand(std::move(toRun), requirements)); } @@ -52,6 +58,13 @@ Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) { Trigger Trigger::WhileActiveContinous( std::function toRun, std::initializer_list requirements) { + return WhileActiveContinous( + std::move(toRun), + wpi::makeArrayRef(requirements.begin(), requirements.end())); +} + +Trigger Trigger::WhileActiveContinous(std::function toRun, + wpi::ArrayRef requirements) { return WhileActiveContinous(InstantCommand(std::move(toRun), requirements)); } @@ -87,6 +100,12 @@ Trigger Trigger::WhenInactive(Command* command, bool interruptible) { Trigger Trigger::WhenInactive(std::function toRun, std::initializer_list requirements) { + return WhenInactive(std::move(toRun), wpi::makeArrayRef(requirements.begin(), + requirements.end())); +} + +Trigger Trigger::WhenInactive(std::function toRun, + wpi::ArrayRef requirements) { return WhenInactive(InstantCommand(std::move(toRun), requirements)); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index 4695c2655e..e50f09e054 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.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. */ @@ -8,6 +8,7 @@ #pragma once #include +#include #include #include @@ -132,7 +133,18 @@ class Command : public frc::ErrorBase { */ SequentialCommandGroup BeforeStarting( std::function toRun, - std::initializer_list requirements = {}) &&; + std::initializer_list requirements) &&; + + /** + * Decorates this command with a runnable to run before this command starts. + * + * @param toRun the Runnable to run + * @param requirements the required subsystems + * @return the decorated command + */ + SequentialCommandGroup BeforeStarting( + std::function toRun, + wpi::ArrayRef requirements = {}) &&; /** * Decorates this command with a runnable to run after the command finishes. @@ -143,7 +155,18 @@ class Command : public frc::ErrorBase { */ SequentialCommandGroup AndThen( std::function toRun, - std::initializer_list requirements = {}) &&; + std::initializer_list requirements) &&; + + /** + * Decorates this command with a runnable to run after the command finishes. + * + * @param toRun the Runnable to run + * @param requirements the required subsystems + * @return the decorated command + */ + SequentialCommandGroup AndThen( + std::function toRun, + wpi::ArrayRef requirements = {}) &&; /** * Decorates this command to run perpetually, ignoring its ordinary end diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h index 81af2f7865..481a51ac84 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.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. */ @@ -12,6 +12,7 @@ #include #include +#include #include #include @@ -32,6 +33,13 @@ class CommandBase : public Command, */ void AddRequirements(std::initializer_list requirements); + /** + * Adds the specified requirements to the command. + * + * @param requirements the requirements to add + */ + void AddRequirements(wpi::ArrayRef requirements); + void AddRequirements(wpi::SmallSet requirements); wpi::SmallSet GetRequirements() const override; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index c114d6a509..f1d0917003 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.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. */ @@ -157,8 +157,10 @@ class CommandScheduler final : public frc::Sendable, void UnregisterSubsystem(Subsystem* subsystem); void RegisterSubsystem(std::initializer_list subsystems); + void RegisterSubsystem(wpi::ArrayRef subsystems); void UnregisterSubsystem(std::initializer_list subsystems); + void UnregisterSubsystem(wpi::ArrayRef subsystems); /** * Sets the default command for a subsystem. Registers that subsystem if it diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h index f85f9be463..eb88b5ebc5 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.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. */ @@ -8,6 +8,9 @@ #pragma once #include +#include + +#include #include "frc2/command/CommandBase.h" #include "frc2/command/CommandHelper.h" @@ -36,7 +39,23 @@ class FunctionalCommand : public CommandHelper { std::function onExecute, std::function onEnd, std::function isFinished, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Creates a new FunctionalCommand. + * + * @param onInit the function to run on command initialization + * @param onExecute the function to run on command execution + * @param onEnd the function to run on command end + * @param isFinished the function that determines whether the command has + * finished + * @param requirements the subsystems required by this command + */ + FunctionalCommand(std::function onInit, + std::function onExecute, + std::function onEnd, + std::function isFinished, + wpi::ArrayRef requirements = {}); FunctionalCommand(FunctionalCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h index 97b4da2408..3f29b17253 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.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,6 +10,8 @@ #include #include +#include + #include "frc2/command/CommandBase.h" #include "frc2/command/CommandHelper.h" @@ -29,7 +31,17 @@ class InstantCommand : public CommandHelper { * @param requirements the subsystems required by this command */ InstantCommand(std::function toRun, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Creates a new InstantCommand that runs the given Runnable with the given + * requirements. + * + * @param toRun the Runnable to run + * @param requirements the subsystems required by this command + */ + InstantCommand(std::function toRun, + wpi::ArrayRef requirements = {}); InstantCommand(InstantCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h index e5a22a5c3f..f667e5fa57 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.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. */ @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -18,6 +19,7 @@ #include #include #include +#include #include "CommandBase.h" #include "CommandHelper.h" @@ -99,6 +101,57 @@ class MecanumControllerCommand output, std::initializer_list requirements); + /** + * Constructs a new MecanumControllerCommand that when executed will follow + * the provided trajectory. PID control and feedforward are handled + * internally. Outputs are scaled from -12 to 12 as a voltage output to the + * motor. + * + *

Note: The controllers will *not* set the outputVolts to zero upon + * completion of the path this is left to the user, since it is not + * appropriate for paths with nonstationary endstates. + * + *

Note 2: The rotation controller will calculate the rotation based on the + * final pose in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose, + * provided by the odometry class. + * @param feedforward The feedforward to use for the drivetrain. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. + * @param frontLeftController The front left wheel velocity PID. + * @param rearLeftController The rear left wheel velocity PID. + * @param frontRightController The front right wheel velocity PID. + * @param rearRightController The rear right wheel velocity PID. + * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing + * the current wheel speeds. + * @param output The output of the velocity PIDs. + * @param requirements The subsystems to require. + */ + MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SimpleMotorFeedforward feedforward, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function currentWheelSpeeds, + frc2::PIDController frontLeftController, + frc2::PIDController rearLeftController, + frc2::PIDController frontRightController, + frc2::PIDController rearRightController, + std::function + output, + wpi::ArrayRef requirements = {}); + /** * Constructs a new MecanumControllerCommand that when executed will follow * the provided trajectory. The user should implement a velocity PID on the @@ -137,6 +190,44 @@ class MecanumControllerCommand output, std::initializer_list requirements); + /** + * Constructs a new MecanumControllerCommand that when executed will follow + * the provided trajectory. The user should implement a velocity PID on the + * desired output wheel velocities. + * + *

Note: The controllers will *not* set the outputVolts to zero upon + * completion of the path - this is left to the user, since it is not + * appropriate for paths with non-stationary end-states. + * + *

Note2: The rotation controller will calculate the rotation based on the + * final pose in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one + * of the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. + * @param output The output of the position PIDs. + * @param requirements The subsystems to require. + */ + MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function + output, + wpi::ArrayRef requirements = {}); + void Initialize() override; void Execute() override; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h index 847c693eff..59af028ff0 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.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. */ @@ -12,6 +12,7 @@ #include #include +#include #include "frc2/command/CommandBase.h" #include "frc2/command/CommandHelper.h" @@ -37,7 +38,17 @@ class NotifierCommand : public CommandHelper { * @param requirements the subsystems required by this command */ NotifierCommand(std::function toRun, units::second_t period, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Creates a new NotifierCommand. + * + * @param toRun the runnable for the notifier to run + * @param period the period at which the notifier should run + * @param requirements the subsystems required by this command + */ + NotifierCommand(std::function toRun, units::second_t period, + wpi::ArrayRef requirements = {}); NotifierCommand(NotifierCommand&& other); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h index 1089fd176d..86533131c8 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.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. */ @@ -40,7 +40,23 @@ class PIDCommand : public CommandHelper { std::function measurementSource, std::function setpointSource, std::function useOutput, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Creates a new PIDCommand, which controls the given output with a + * PIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param setpointSource the controller's reference (aka setpoint) + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + PIDCommand(PIDController controller, + std::function measurementSource, + std::function setpointSource, + std::function useOutput, + wpi::ArrayRef requirements = {}); /** * Creates a new PIDCommand, which controls the given output with a @@ -57,6 +73,21 @@ class PIDCommand : public CommandHelper { std::function useOutput, std::initializer_list requirements); + /** + * Creates a new PIDCommand, which controls the given output with a + * PIDController with a constant setpoint. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param setpoint the controller's setpoint (aka setpoint) + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + PIDCommand(PIDController controller, + std::function measurementSource, double setpoint, + std::function useOutput, + wpi::ArrayRef requirements = {}); + PIDCommand(PIDCommand&& other) = default; PIDCommand(const PIDCommand& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h index c2ad7444e4..b685230165 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h @@ -13,6 +13,7 @@ #include #include +#include #include "frc2/command/CommandBase.h" #include "frc2/command/CommandHelper.h" @@ -50,7 +51,29 @@ class ProfiledPIDCommand std::function measurementSource, std::function goalSource, std::function useOutput, - std::initializer_list requirements = {}) + std::initializer_list requirements) + : m_controller{controller}, + m_measurement{std::move(measurementSource)}, + m_goal{std::move(goalSource)}, + m_useOutput{std::move(useOutput)} { + this->AddRequirements(requirements); + } + + /** + * Creates a new PIDCommand, which controls the given output with a + * ProfiledPIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goalSource the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + ProfiledPIDCommand(frc::ProfiledPIDController controller, + std::function measurementSource, + std::function goalSource, + std::function useOutput, + wpi::ArrayRef requirements = {}) : m_controller{controller}, m_measurement{std::move(measurementSource)}, m_goal{std::move(goalSource)}, @@ -79,6 +102,27 @@ class ProfiledPIDCommand }, useOutput, requirements) {} + /** + * Creates a new PIDCommand, which controls the given output with a + * ProfiledPIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goalSource the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + ProfiledPIDCommand(frc::ProfiledPIDController controller, + std::function measurementSource, + std::function goalSource, + std::function useOutput, + wpi::ArrayRef requirements = {}) + : ProfiledPIDCommand(controller, measurementSource, + [&goalSource]() { + return State{goalSource(), Velocity_t{0}}; + }, + useOutput, requirements) {} + /** * Creates a new PIDCommand, which controls the given output with a * ProfiledPIDController with a constant goal. @@ -96,6 +140,23 @@ class ProfiledPIDCommand : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; }, useOutput, requirements) {} + /** + * Creates a new PIDCommand, which controls the given output with a + * ProfiledPIDController with a constant goal. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goal the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + ProfiledPIDCommand(frc::ProfiledPIDController controller, + std::function measurementSource, State goal, + std::function useOutput, + wpi::ArrayRef requirements = {}) + : ProfiledPIDCommand(controller, measurementSource, + [goal] { return goal; }, useOutput, requirements) {} + /** * Creates a new PIDCommand, which controls the given output with a * ProfiledPIDController with a constant goal. @@ -114,6 +175,24 @@ class ProfiledPIDCommand : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; }, useOutput, requirements) {} + /** + * Creates a new PIDCommand, which controls the given output with a + * ProfiledPIDController with a constant goal. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goal the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + ProfiledPIDCommand(frc::ProfiledPIDController controller, + std::function measurementSource, + Distance_t goal, + std::function useOutput, + wpi::ArrayRef requirements = {}) + : ProfiledPIDCommand(controller, measurementSource, + [goal] { return goal; }, useOutput, requirements) {} + ProfiledPIDCommand(ProfiledPIDCommand&& other) = default; ProfiledPIDCommand(const ProfiledPIDCommand& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h index 60d1b7e90f..580bc574af 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.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. */ @@ -18,6 +18,7 @@ #include #include #include +#include #include "frc2/Timer.h" #include "frc2/command/CommandBase.h" @@ -78,7 +79,44 @@ class RamseteCommand : public CommandHelper { frc2::PIDController leftController, frc2::PIDController rightController, std::function output, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Constructs a new RamseteCommand that, when executed, will follow the + * provided trajectory. PID control and feedforward are handled internally, + * and outputs are scaled -12 to 12 representing units of volts. + * + *

Note: The controller will *not* set the outputVolts to zero upon + * completion of the path - this is left to the user, since it is not + * appropriate for paths with nonstationary endstates. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param controller The RAMSETE controller used to follow the + * trajectory. + * @param feedforward A component for calculating the feedforward for the + * drive. + * @param kinematics The kinematics for the robot drivetrain. + * @param wheelSpeeds A function that supplies the speeds of the left + * and right sides of the robot drive. + * @param leftController The PIDController for the left side of the robot + * drive. + * @param rightController The PIDController for the right side of the robot + * drive. + * @param output A function that consumes the computed left and right + * outputs (in volts) for the robot drive. + * @param requirements The subsystems to require. + */ + RamseteCommand(frc::Trajectory trajectory, std::function pose, + frc::RamseteController controller, + frc::SimpleMotorFeedforward feedforward, + frc::DifferentialDriveKinematics kinematics, + std::function wheelSpeeds, + frc2::PIDController leftController, + frc2::PIDController rightController, + std::function output, + wpi::ArrayRef requirements = {}); /** * Constructs a new RamseteCommand that, when executed, will follow the @@ -104,6 +142,30 @@ class RamseteCommand : public CommandHelper { output, std::initializer_list requirements); + /** + * Constructs a new RamseteCommand that, when executed, will follow the + * provided trajectory. Performs no PID control and calculates no + * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller, + * and will need to be converted into a usable form by the user. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param controller The RAMSETE controller used to follow the + * trajectory. + * @param kinematics The kinematics for the robot drivetrain. + * @param output A function that consumes the computed left and right + * wheel speeds. + * @param requirements The subsystems to require. + */ + RamseteCommand(frc::Trajectory trajectory, std::function pose, + frc::RamseteController controller, + frc::DifferentialDriveKinematics kinematics, + std::function + output, + wpi::ArrayRef requirements = {}); + void Initialize() override; void Execute() override; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h index aae7b74cf1..20362303bc 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.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,6 +10,8 @@ #include #include +#include + #include "frc2/command/CommandBase.h" #include "frc2/command/CommandHelper.h" @@ -30,7 +32,17 @@ class RunCommand : public CommandHelper { * @param requirements the subsystems to require */ RunCommand(std::function toRun, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Creates a new RunCommand. The Runnable will be run continuously until the + * command ends. Does not run when disabled. + * + * @param toRun the Runnable to run + * @param requirements the subsystems to require + */ + RunCommand(std::function toRun, + wpi::ArrayRef requirements = {}); RunCommand(RunCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h index 32b7e76abc..c0c9096b5e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.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,6 +10,8 @@ #include #include +#include + #include "frc2/command/CommandBase.h" #include "frc2/command/CommandHelper.h" @@ -32,7 +34,18 @@ class StartEndCommand : public CommandHelper { * @param requirements the subsystems required by this command */ StartEndCommand(std::function onInit, std::function onEnd, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Creates a new StartEndCommand. Will run the given runnables when the + * command starts and when it ends. + * + * @param onInit the Runnable to run on command init + * @param onEnd the Runnable to run on command end + * @param requirements the subsystems required by this command + */ + StartEndCommand(std::function onInit, std::function onEnd, + wpi::ArrayRef requirements = {}); StartEndCommand(StartEndCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h index ab3fe10ae9..38ca60cd3e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.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. */ @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -17,6 +18,7 @@ #include #include #include +#include #include "CommandBase.h" #include "CommandHelper.h" @@ -92,6 +94,42 @@ class SwerveControllerCommand output, std::initializer_list requirements); + /** + * Constructs a new SwerveControllerCommand that when executed will follow the + * provided trajectory. This command will not return output voltages but + * rather raw module states from the position controllers which need to be put + * into a velocity PID. + * + *

Note: The controllers will *not* set the outputVolts to zero upon + * completion of the path- this is left to the user, since it is not + * appropriate for paths with nonstationary endstates. + * + *

Note 2: The rotation controller will calculate the rotation based on the + * final pose in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose, + * provided by the odometry class. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param output The raw output module states from the + * position controllers. + * @param requirements The subsystems to require. + */ + SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc2::PIDController xController, frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function)> + output, + wpi::ArrayRef requirements = {}); + void Initialize() override; void Execute() override; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc index 7beafefdfc..42d726d316 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc @@ -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. */ @@ -31,6 +31,26 @@ SwerveControllerCommand::SwerveControllerCommand( this->AddRequirements(requirements); } +template +SwerveControllerCommand::SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc2::PIDController xController, frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function)> output, + wpi::ArrayRef requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_kinematics(kinematics), + m_xController(std::make_unique(xController)), + m_yController(std::make_unique(yController)), + m_thetaController( + std::make_unique>( + thetaController)), + m_outputStates(output) { + this->AddRequirements(requirements); +} + template void SwerveControllerCommand::Initialize() { m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h index 6e04e9b96c..a83dc39a37 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.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. */ @@ -11,6 +11,7 @@ #include #include +#include #include "frc2/Timer.h" #include "frc2/command/CommandBase.h" @@ -42,7 +43,21 @@ class TrapezoidProfileCommand */ TrapezoidProfileCommand(frc::TrapezoidProfile profile, std::function output, - std::initializer_list requirements = {}) + std::initializer_list requirements) + : m_profile(profile), m_output(output) { + this->AddRequirements(requirements); + } + + /** + * Creates a new TrapezoidProfileCommand that will execute the given + * TrapezoidalProfile. Output will be piped to the provided consumer function. + * + * @param profile The motion profile to execute. + * @param output The consumer for the profile output. + */ + TrapezoidProfileCommand(frc::TrapezoidProfile profile, + std::function output, + wpi::ArrayRef requirements = {}) : m_profile(profile), m_output(output) { this->AddRequirements(requirements); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h index 6561652011..af861d442a 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h @@ -1,13 +1,18 @@ /*----------------------------------------------------------------------------*/ -/* 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. */ /*----------------------------------------------------------------------------*/ #pragma once + +#include +#include #include +#include + #include "Trigger.h" namespace frc2 { @@ -68,7 +73,16 @@ class Button : public Trigger { * @param requirements the required subsystems. */ Button WhenPressed(std::function toRun, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Binds a runnable to execute when the button is pressed. + * + * @param toRun the runnable to execute. + * @param requirements the required subsystems. + */ + Button WhenPressed(std::function toRun, + wpi::ArrayRef requirements = {}); /** * Binds a command to be started repeatedly while the button is pressed, and @@ -105,7 +119,16 @@ class Button : public Trigger { * @param requirements the required subsystems. */ Button WhileHeld(std::function toRun, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Binds a runnable to execute repeatedly while the button is pressed. + * + * @param toRun the runnable to execute. + * @param requirements the required subsystems. + */ + Button WhileHeld(std::function toRun, + wpi::ArrayRef requirements = {}); /** * Binds a command to be started when the button is pressed, and cancelled @@ -170,7 +193,16 @@ class Button : public Trigger { * @param requirements the required subsystems. */ Button WhenReleased(std::function toRun, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Binds a runnable to execute when the button is released. + * + * @param toRun the runnable to execute. + * @param requirements the required subsystems. + */ + Button WhenReleased(std::function toRun, + wpi::ArrayRef requirements = {}); /** * Binds a command to start when the button is pressed, and be cancelled when diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h index efc4bc0263..7df6b4ecbb 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.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. */ @@ -7,10 +7,13 @@ #pragma once -#include +#include +#include #include #include +#include + #include "frc2/command/Command.h" #include "frc2/command/CommandScheduler.h" @@ -99,7 +102,16 @@ class Trigger { * @paaram requirements the required subsystems. */ Trigger WhenActive(std::function toRun, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Binds a runnable to execute when the trigger becomes active. + * + * @param toRun the runnable to execute. + * @paaram requirements the required subsystems. + */ + Trigger WhenActive(std::function toRun, + wpi::ArrayRef requirements = {}); /** * Binds a command to be started repeatedly while the trigger is active, and @@ -149,9 +161,17 @@ class Trigger { * @param toRun the runnable to execute. * @param requirements the required subsystems. */ - Trigger WhileActiveContinous( - std::function toRun, - std::initializer_list requirements = {}); + Trigger WhileActiveContinous(std::function toRun, + std::initializer_list requirements); + + /** + * Binds a runnable to execute repeatedly while the trigger is active. + * + * @param toRun the runnable to execute. + * @param requirements the required subsystems. + */ + Trigger WhileActiveContinous(std::function toRun, + wpi::ArrayRef requirements = {}); /** * Binds a command to be started when the trigger becomes active, and @@ -242,7 +262,16 @@ class Trigger { * @param requirements the required subsystems. */ Trigger WhenInactive(std::function toRun, - std::initializer_list requirements = {}); + std::initializer_list requirements); + + /** + * Binds a runnable to execute when the trigger becomes inactive. + * + * @param toRun the runnable to execute. + * @param requirements the required subsystems. + */ + Trigger WhenInactive(std::function toRun, + wpi::ArrayRef requirements = {}); /** * Binds a command to start when the trigger becomes active, and be cancelled