mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
455 lines
12 KiB
C++
455 lines
12 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2011-2018 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. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "Commands/Command.h"
|
|
|
|
#include <typeinfo>
|
|
|
|
#include "Commands/CommandGroup.h"
|
|
#include "Commands/Scheduler.h"
|
|
#include "LiveWindow/LiveWindow.h"
|
|
#include "RobotState.h"
|
|
#include "SmartDashboard/SendableBuilder.h"
|
|
#include "Timer.h"
|
|
#include "WPIErrors.h"
|
|
|
|
using namespace frc;
|
|
|
|
int Command::m_commandCounter = 0;
|
|
|
|
/**
|
|
* Creates a new command.
|
|
*
|
|
* The name of this command will be default.
|
|
*/
|
|
Command::Command() : Command("", -1.0) {}
|
|
|
|
/**
|
|
* Creates a new command with the given name and no timeout.
|
|
*
|
|
* @param name the name for this command
|
|
*/
|
|
Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
|
|
|
|
/**
|
|
* Creates a new command with the given timeout and a default name.
|
|
*
|
|
* @param timeout the time (in seconds) before this command "times out"
|
|
* @see IsTimedOut()
|
|
*/
|
|
Command::Command(double timeout) : Command("", timeout) {}
|
|
|
|
/**
|
|
* Creates a new command with the given name and timeout.
|
|
*
|
|
* @param name the name of the command
|
|
* @param timeout the time (in seconds) before this command "times out"
|
|
* @see IsTimedOut()
|
|
*/
|
|
Command::Command(const wpi::Twine& name, double timeout) : SendableBase(false) {
|
|
// We use -1.0 to indicate no timeout.
|
|
if (timeout < 0.0 && timeout != -1.0)
|
|
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
|
|
|
|
m_timeout = timeout;
|
|
|
|
// If name contains an empty string
|
|
if (name.isTriviallyEmpty() ||
|
|
(name.isSingleStringRef() && name.getSingleStringRef().empty())) {
|
|
SetName("Command_" + wpi::Twine(typeid(*this).name()));
|
|
} else {
|
|
SetName(name);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the ID (sequence number) for this command.
|
|
*
|
|
* The ID is a unique sequence number that is incremented for each command.
|
|
*
|
|
* @return The ID of this command
|
|
*/
|
|
int Command::GetID() const { return m_commandID; }
|
|
|
|
/**
|
|
* Sets the timeout of this command.
|
|
*
|
|
* @param timeout the timeout (in seconds)
|
|
* @see IsTimedOut()
|
|
*/
|
|
void Command::SetTimeout(double timeout) {
|
|
if (timeout < 0.0)
|
|
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
|
|
else
|
|
m_timeout = timeout;
|
|
}
|
|
|
|
/**
|
|
* Returns the time since this command was initialized (in seconds).
|
|
*
|
|
* This function will work even if there is no specified timeout.
|
|
*
|
|
* @return the time since this command was initialized (in seconds).
|
|
*/
|
|
double Command::TimeSinceInitialized() const {
|
|
if (m_startTime < 0.0)
|
|
return 0.0;
|
|
else
|
|
return Timer::GetFPGATimestamp() - m_startTime;
|
|
}
|
|
|
|
/**
|
|
* This method specifies that the given Subsystem is used by this command.
|
|
*
|
|
* This method is crucial to the functioning of the Command System in general.
|
|
*
|
|
* Note that the recommended way to call this method is in the constructor.
|
|
*
|
|
* @param subsystem The Subsystem required
|
|
* @see Subsystem
|
|
*/
|
|
void Command::Requires(Subsystem* subsystem) {
|
|
if (!AssertUnlocked("Can not add new requirement to command")) return;
|
|
|
|
if (subsystem != nullptr)
|
|
m_requirements.insert(subsystem);
|
|
else
|
|
wpi_setWPIErrorWithContext(NullParameter, "subsystem");
|
|
}
|
|
|
|
/**
|
|
* Called when the command has been removed.
|
|
*
|
|
* This will call Interrupted() or End().
|
|
*/
|
|
void Command::Removed() {
|
|
if (m_initialized) {
|
|
if (IsCanceled()) {
|
|
Interrupted();
|
|
_Interrupted();
|
|
} else {
|
|
End();
|
|
_End();
|
|
}
|
|
}
|
|
m_initialized = false;
|
|
m_canceled = false;
|
|
m_running = false;
|
|
m_completed = true;
|
|
}
|
|
|
|
/**
|
|
* Starts up the command. Gets the command ready to start.
|
|
*
|
|
* Note that the command will eventually start, however it will not necessarily
|
|
* do so immediately, and may in fact be canceled before initialize is even
|
|
* called.
|
|
*/
|
|
void Command::Start() {
|
|
LockChanges();
|
|
if (m_parent != nullptr)
|
|
wpi_setWPIErrorWithContext(
|
|
CommandIllegalUse,
|
|
"Can not start a command that is part of a command group");
|
|
|
|
m_completed = false;
|
|
Scheduler::GetInstance()->AddCommand(this);
|
|
}
|
|
|
|
/**
|
|
* The run method is used internally to actually run the commands.
|
|
*
|
|
* @return Whether or not the command should stay within the Scheduler.
|
|
*/
|
|
bool Command::Run() {
|
|
if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
|
|
Cancel();
|
|
|
|
if (IsCanceled()) return false;
|
|
|
|
if (!m_initialized) {
|
|
m_initialized = true;
|
|
StartTiming();
|
|
_Initialize();
|
|
Initialize();
|
|
}
|
|
_Execute();
|
|
Execute();
|
|
return !IsFinished();
|
|
}
|
|
|
|
/**
|
|
* The initialize method is called the first time this Command is run after
|
|
* being started.
|
|
*/
|
|
void Command::Initialize() {}
|
|
|
|
/**
|
|
* The execute method is called repeatedly until this Command either finishes
|
|
* or is canceled.
|
|
*/
|
|
void Command::Execute() {}
|
|
|
|
/**
|
|
* Called when the command ended peacefully. This is where you may want to wrap
|
|
* up loose ends, like shutting off a motor that was being used in the command.
|
|
*/
|
|
void Command::End() {}
|
|
|
|
/**
|
|
* Called when the command ends because somebody called Cancel() or another
|
|
* command shared the same requirements as this one, and booted it out.
|
|
*
|
|
* This is where you may want to wrap up loose ends, like shutting off a motor
|
|
* that was being used in the command.
|
|
*
|
|
* Generally, it is useful to simply call the End() method within this method,
|
|
* as done here.
|
|
*/
|
|
void Command::Interrupted() { End(); }
|
|
|
|
void Command::_Initialize() { m_completed = false; }
|
|
|
|
void Command::_Interrupted() { m_completed = true; }
|
|
|
|
void Command::_Execute() {}
|
|
|
|
void Command::_End() { m_completed = true; }
|
|
|
|
/**
|
|
* Called to indicate that the timer should start.
|
|
*
|
|
* This is called right before Initialize() is, inside the Run() method.
|
|
*/
|
|
void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
|
|
|
|
/**
|
|
* Returns whether or not the TimeSinceInitialized() method returns a number
|
|
* which is greater than or equal to the timeout for the command.
|
|
*
|
|
* If there is no timeout, this will always return false.
|
|
*
|
|
* @return whether the time has expired
|
|
*/
|
|
bool Command::IsTimedOut() const {
|
|
return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
|
|
}
|
|
|
|
/**
|
|
* Returns the requirements (as an std::set of Subsystem pointers) of this
|
|
* command.
|
|
*
|
|
* @return The requirements (as an std::set of Subsystem pointers) of this
|
|
* command
|
|
*/
|
|
Command::SubsystemSet Command::GetRequirements() const {
|
|
return m_requirements;
|
|
}
|
|
|
|
/**
|
|
* Prevents further changes from being made.
|
|
*/
|
|
void Command::LockChanges() { m_locked = true; }
|
|
|
|
/**
|
|
* If changes are locked, then this will generate a CommandIllegalUse error.
|
|
*
|
|
* @param message The message to report on error (it is appended by a default
|
|
* message)
|
|
* @return True if assert passed, false if assert failed.
|
|
*/
|
|
bool Command::AssertUnlocked(const std::string& message) {
|
|
if (m_locked) {
|
|
std::string buf =
|
|
message + " after being started or being added to a command group";
|
|
wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Sets the parent of this command. No actual change is made to the group.
|
|
*
|
|
* @param parent the parent
|
|
*/
|
|
void Command::SetParent(CommandGroup* parent) {
|
|
if (parent == nullptr) {
|
|
wpi_setWPIErrorWithContext(NullParameter, "parent");
|
|
} else if (m_parent != nullptr) {
|
|
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
|
"Can not give command to a command group after "
|
|
"already being put in a command group");
|
|
} else {
|
|
LockChanges();
|
|
m_parent = parent;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Returns whether the command has a parent.
|
|
*
|
|
* @param True if the command has a parent.
|
|
*/
|
|
bool Command::IsParented() const { return m_parent != nullptr; }
|
|
|
|
/**
|
|
* Clears list of subsystem requirements.
|
|
*
|
|
* This is only used by ConditionalCommand so cancelling the chosen command
|
|
* works properly in CommandGroup.
|
|
*/
|
|
void Command::ClearRequirements() { m_requirements.clear(); }
|
|
|
|
/**
|
|
* This is used internally to mark that the command has been started.
|
|
*
|
|
* The lifecycle of a command is:
|
|
*
|
|
* StartRunning() is called. Run() is called (multiple times potentially).
|
|
* Removed() is called.
|
|
*
|
|
* It is very important that StartRunning() and Removed() be called in order or
|
|
* some assumptions of the code will be broken.
|
|
*/
|
|
void Command::StartRunning() {
|
|
m_running = true;
|
|
m_startTime = -1;
|
|
m_completed = false;
|
|
}
|
|
|
|
/**
|
|
* Returns whether or not the command is running.
|
|
*
|
|
* This may return true even if the command has just been canceled, as it may
|
|
* not have yet called Interrupted().
|
|
*
|
|
* @return whether or not the command is running
|
|
*/
|
|
bool Command::IsRunning() const { return m_running; }
|
|
|
|
/**
|
|
* Returns whether or not the command has been initialized.
|
|
*
|
|
* @return whether or not the command has been initialized.
|
|
*/
|
|
bool Command::IsInitialized() const { return m_initialized; }
|
|
|
|
/**
|
|
* Returns whether or not the command has completed running.
|
|
*
|
|
* @return whether or not the command has completed running.
|
|
*/
|
|
bool Command::IsCompleted() const { return m_completed; }
|
|
|
|
/**
|
|
* This will cancel the current command.
|
|
*
|
|
* This will cancel the current command eventually. It can be called multiple
|
|
* times. And it can be called when the command is not running. If the command
|
|
* is running though, then the command will be marked as canceled and eventually
|
|
* removed.
|
|
*
|
|
* A command can not be canceled if it is a part of a command group, you must
|
|
* cancel the command group instead.
|
|
*/
|
|
void Command::Cancel() {
|
|
if (m_parent != nullptr)
|
|
wpi_setWPIErrorWithContext(
|
|
CommandIllegalUse,
|
|
"Can not cancel a command that is part of a command group");
|
|
|
|
_Cancel();
|
|
}
|
|
|
|
/**
|
|
* This works like Cancel(), except that it doesn't throw an exception if it is
|
|
* a part of a command group.
|
|
*
|
|
* Should only be called by the parent command group.
|
|
*/
|
|
void Command::_Cancel() {
|
|
if (IsRunning()) m_canceled = true;
|
|
}
|
|
|
|
/**
|
|
* Returns whether or not this has been canceled.
|
|
*
|
|
* @return whether or not this has been canceled
|
|
*/
|
|
bool Command::IsCanceled() const { return m_canceled; }
|
|
|
|
/**
|
|
* Returns whether or not this command can be interrupted.
|
|
*
|
|
* @return whether or not this command can be interrupted
|
|
*/
|
|
bool Command::IsInterruptible() const { return m_interruptible; }
|
|
|
|
/**
|
|
* Sets whether or not this command can be interrupted.
|
|
*
|
|
* @param interruptible whether or not this command can be interrupted
|
|
*/
|
|
void Command::SetInterruptible(bool interruptible) {
|
|
m_interruptible = interruptible;
|
|
}
|
|
|
|
/**
|
|
* Checks if the command requires the given Subsystem.
|
|
*
|
|
* @param system the system
|
|
* @return whether or not the subsystem is required (false if given nullptr)
|
|
*/
|
|
bool Command::DoesRequire(Subsystem* system) const {
|
|
return m_requirements.count(system) > 0;
|
|
}
|
|
|
|
/**
|
|
* Returns the CommandGroup that this command is a part of.
|
|
*
|
|
* Will return null if this Command is not in a group.
|
|
*
|
|
* @return The CommandGroup that this command is a part of (or null if not in
|
|
* group)
|
|
*/
|
|
CommandGroup* Command::GetGroup() const { return m_parent; }
|
|
|
|
/**
|
|
* Sets whether or not this Command should run when the robot is disabled.
|
|
*
|
|
* By default a command will not run when the robot is disabled, and will in
|
|
* fact be canceled.
|
|
*
|
|
* @param run Whether this command should run when the robot is disabled.
|
|
*/
|
|
void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
|
|
|
|
/**
|
|
* Returns whether or not this Command will run when the robot is disabled, or
|
|
* if it will cancel itself.
|
|
*
|
|
* @return Whether this Command will run when the robot is disabled, or if it
|
|
* will cancel itself.
|
|
*/
|
|
bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
|
|
|
|
void Command::InitSendable(SendableBuilder& builder) {
|
|
builder.SetSmartDashboardType("Command");
|
|
builder.AddStringProperty(".name", [=]() { return GetName(); }, nullptr);
|
|
builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
|
|
[=](bool value) {
|
|
if (value) {
|
|
if (!IsRunning()) Start();
|
|
} else {
|
|
if (IsRunning()) Cancel();
|
|
}
|
|
});
|
|
builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
|
|
nullptr);
|
|
}
|