mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
Split the two command implementations into separate libraries (#2012)
This will allow us at the user code side to determine to include old commands, new commands or both.
This commit is contained in:
Thad House
committed by
Peter Johnson
Peter Johnson
parent
2ad15cae19
commit
509819d83f
@@ -16,7 +16,6 @@
|
||||
|
||||
#include "frc/DriverStation.h"
|
||||
#include "frc/Timer.h"
|
||||
#include "frc/commands/Scheduler.h"
|
||||
#include "frc/livewindow/LiveWindow.h"
|
||||
#include "frc/shuffleboard/Shuffleboard.h"
|
||||
#include "frc/smartdashboard/SmartDashboard.h"
|
||||
@@ -131,7 +130,6 @@ void IterativeRobotBase::LoopFunc() {
|
||||
TeleopInit();
|
||||
m_watchdog.AddEpoch("TeleopInit()");
|
||||
m_lastMode = Mode::kTeleop;
|
||||
Scheduler::GetInstance()->SetEnabled(true);
|
||||
}
|
||||
|
||||
HAL_ObserveUserProgramTeleop();
|
||||
|
||||
@@ -1,20 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/Button.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
void Button::WhenPressed(Command* command) { WhenActive(command); }
|
||||
|
||||
void Button::WhileHeld(Command* command) { WhileActive(command); }
|
||||
|
||||
void Button::WhenReleased(Command* command) { WhenInactive(command); }
|
||||
|
||||
void Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); }
|
||||
|
||||
void Button::ToggleWhenPressed(Command* command) { ToggleWhenActive(command); }
|
||||
@@ -1,17 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/ButtonScheduler.h"
|
||||
|
||||
#include "frc/commands/Scheduler.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders)
|
||||
: m_pressedLast(last), m_button(button), m_command(orders) {}
|
||||
|
||||
void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); }
|
||||
@@ -1,27 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/CancelButtonScheduler.h"
|
||||
|
||||
#include "frc/buttons/Button.h"
|
||||
#include "frc/commands/Command.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger* button,
|
||||
Command* orders)
|
||||
: ButtonScheduler(last, button, orders) {}
|
||||
|
||||
void CancelButtonScheduler::Execute() {
|
||||
bool pressed = m_button->Grab();
|
||||
|
||||
if (!m_pressedLast && pressed) {
|
||||
m_command->Cancel();
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
@@ -1,29 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/HeldButtonScheduler.h"
|
||||
|
||||
#include "frc/buttons/Button.h"
|
||||
#include "frc/commands/Command.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger* button,
|
||||
Command* orders)
|
||||
: ButtonScheduler(last, button, orders) {}
|
||||
|
||||
void HeldButtonScheduler::Execute() {
|
||||
bool pressed = m_button->Grab();
|
||||
|
||||
if (pressed) {
|
||||
m_command->Start();
|
||||
} else if (m_pressedLast && !pressed) {
|
||||
m_command->Cancel();
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/InternalButton.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
InternalButton::InternalButton(bool inverted)
|
||||
: m_pressed(inverted), m_inverted(inverted) {}
|
||||
|
||||
void InternalButton::SetInverted(bool inverted) { m_inverted = inverted; }
|
||||
|
||||
void InternalButton::SetPressed(bool pressed) { m_pressed = pressed; }
|
||||
|
||||
bool InternalButton::Get() { return m_pressed ^ m_inverted; }
|
||||
@@ -1,15 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/JoystickButton.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber)
|
||||
: m_joystick(joystick), m_buttonNumber(buttonNumber) {}
|
||||
|
||||
bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); }
|
||||
@@ -1,26 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/NetworkButton.h"
|
||||
|
||||
#include <networktables/NetworkTable.h>
|
||||
#include <networktables/NetworkTableInstance.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
NetworkButton::NetworkButton(const wpi::Twine& tableName,
|
||||
const wpi::Twine& field)
|
||||
: NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(tableName),
|
||||
field) {}
|
||||
|
||||
NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
|
||||
const wpi::Twine& field)
|
||||
: m_entry(table->GetEntry(field)) {}
|
||||
|
||||
bool NetworkButton::Get() {
|
||||
return m_entry.GetInstance().IsConnected() && m_entry.GetBoolean(false);
|
||||
}
|
||||
@@ -1,15 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 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 "frc/buttons/POVButton.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
POVButton::POVButton(GenericHID& joystick, int angle, int povNumber)
|
||||
: m_joystick(&joystick), m_angle(angle), m_povNumber(povNumber) {}
|
||||
|
||||
bool POVButton::Get() { return m_joystick->GetPOV(m_povNumber) == m_angle; }
|
||||
@@ -1,27 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/PressedButtonScheduler.h"
|
||||
|
||||
#include "frc/buttons/Button.h"
|
||||
#include "frc/commands/Command.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger* button,
|
||||
Command* orders)
|
||||
: ButtonScheduler(last, button, orders) {}
|
||||
|
||||
void PressedButtonScheduler::Execute() {
|
||||
bool pressed = m_button->Grab();
|
||||
|
||||
if (!m_pressedLast && pressed) {
|
||||
m_command->Start();
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
@@ -1,27 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/ReleasedButtonScheduler.h"
|
||||
|
||||
#include "frc/buttons/Button.h"
|
||||
#include "frc/commands/Command.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger* button,
|
||||
Command* orders)
|
||||
: ButtonScheduler(last, button, orders) {}
|
||||
|
||||
void ReleasedButtonScheduler::Execute() {
|
||||
bool pressed = m_button->Grab();
|
||||
|
||||
if (m_pressedLast && !pressed) {
|
||||
m_command->Start();
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
@@ -1,31 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/buttons/ToggleButtonScheduler.h"
|
||||
|
||||
#include "frc/buttons/Button.h"
|
||||
#include "frc/commands/Command.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger* button,
|
||||
Command* orders)
|
||||
: ButtonScheduler(last, button, orders) {}
|
||||
|
||||
void ToggleButtonScheduler::Execute() {
|
||||
bool pressed = m_button->Grab();
|
||||
|
||||
if (!m_pressedLast && pressed) {
|
||||
if (m_command->IsRunning()) {
|
||||
m_command->Cancel();
|
||||
} else {
|
||||
m_command->Start();
|
||||
}
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
@@ -1,71 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2011-2019 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 "frc/buttons/Button.h"
|
||||
#include "frc/buttons/CancelButtonScheduler.h"
|
||||
#include "frc/buttons/HeldButtonScheduler.h"
|
||||
#include "frc/buttons/PressedButtonScheduler.h"
|
||||
#include "frc/buttons/ReleasedButtonScheduler.h"
|
||||
#include "frc/buttons/ToggleButtonScheduler.h"
|
||||
#include "frc/smartdashboard/SendableBuilder.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Trigger::Trigger(const Trigger& rhs) : SendableHelper(rhs) {}
|
||||
|
||||
Trigger& Trigger::operator=(const Trigger& rhs) {
|
||||
SendableHelper::operator=(rhs);
|
||||
m_sendablePressed = false;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Trigger::Trigger(Trigger&& rhs)
|
||||
: SendableHelper(std::move(rhs)),
|
||||
m_sendablePressed(rhs.m_sendablePressed.load()) {
|
||||
rhs.m_sendablePressed = false;
|
||||
}
|
||||
|
||||
Trigger& Trigger::operator=(Trigger&& rhs) {
|
||||
SendableHelper::operator=(std::move(rhs));
|
||||
m_sendablePressed = rhs.m_sendablePressed.load();
|
||||
rhs.m_sendablePressed = false;
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Trigger::Grab() { return Get() || m_sendablePressed; }
|
||||
|
||||
void Trigger::WhenActive(Command* command) {
|
||||
auto pbs = new PressedButtonScheduler(Grab(), this, command);
|
||||
pbs->Start();
|
||||
}
|
||||
|
||||
void Trigger::WhileActive(Command* command) {
|
||||
auto hbs = new HeldButtonScheduler(Grab(), this, command);
|
||||
hbs->Start();
|
||||
}
|
||||
|
||||
void Trigger::WhenInactive(Command* command) {
|
||||
auto rbs = new ReleasedButtonScheduler(Grab(), this, command);
|
||||
rbs->Start();
|
||||
}
|
||||
|
||||
void Trigger::CancelWhenActive(Command* command) {
|
||||
auto cbs = new CancelButtonScheduler(Grab(), this, command);
|
||||
cbs->Start();
|
||||
}
|
||||
|
||||
void Trigger::ToggleWhenActive(Command* command) {
|
||||
auto tbs = new ToggleButtonScheduler(Grab(), this, command);
|
||||
tbs->Start();
|
||||
}
|
||||
|
||||
void Trigger::InitSendable(SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Button");
|
||||
builder.SetSafeState([=]() { m_sendablePressed = false; });
|
||||
builder.AddBooleanProperty("pressed", [=]() { return Grab(); },
|
||||
[=](bool value) { m_sendablePressed = value; });
|
||||
}
|
||||
@@ -1,264 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2011-2019 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 "frc/commands/Command.h"
|
||||
|
||||
#include <typeinfo>
|
||||
|
||||
#include "frc/RobotState.h"
|
||||
#include "frc/Timer.h"
|
||||
#include "frc/WPIErrors.h"
|
||||
#include "frc/commands/CommandGroup.h"
|
||||
#include "frc/commands/Scheduler.h"
|
||||
#include "frc/livewindow/LiveWindow.h"
|
||||
#include "frc/smartdashboard/SendableBuilder.h"
|
||||
#include "frc/smartdashboard/SendableRegistry.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
int Command::m_commandCounter = 0;
|
||||
|
||||
Command::Command() : Command("", -1.0) {}
|
||||
|
||||
Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
|
||||
|
||||
Command::Command(double timeout) : Command("", timeout) {}
|
||||
|
||||
Command::Command(Subsystem& subsystem) : Command("", -1.0) {
|
||||
Requires(&subsystem);
|
||||
}
|
||||
|
||||
Command::Command(const wpi::Twine& name, double timeout) {
|
||||
// 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())) {
|
||||
SendableRegistry::GetInstance().Add(
|
||||
this, "Command_" + wpi::Twine(typeid(*this).name()));
|
||||
} else {
|
||||
SendableRegistry::GetInstance().Add(this, name);
|
||||
}
|
||||
}
|
||||
|
||||
Command::Command(const wpi::Twine& name, Subsystem& subsystem)
|
||||
: Command(name, -1.0) {
|
||||
Requires(&subsystem);
|
||||
}
|
||||
|
||||
Command::Command(double timeout, Subsystem& subsystem) : Command("", timeout) {
|
||||
Requires(&subsystem);
|
||||
}
|
||||
|
||||
Command::Command(const wpi::Twine& name, double timeout, Subsystem& subsystem)
|
||||
: Command(name, timeout) {
|
||||
Requires(&subsystem);
|
||||
}
|
||||
|
||||
double Command::TimeSinceInitialized() const {
|
||||
if (m_startTime < 0.0)
|
||||
return 0.0;
|
||||
else
|
||||
return Timer::GetFPGATimestamp() - m_startTime;
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
void Command::Cancel() {
|
||||
if (m_parent != nullptr)
|
||||
wpi_setWPIErrorWithContext(
|
||||
CommandIllegalUse,
|
||||
"Can not cancel a command that is part of a command group");
|
||||
|
||||
_Cancel();
|
||||
}
|
||||
|
||||
bool Command::IsRunning() const { return m_running; }
|
||||
|
||||
bool Command::IsInitialized() const { return m_initialized; }
|
||||
|
||||
bool Command::IsCompleted() const { return m_completed; }
|
||||
|
||||
bool Command::IsCanceled() const { return m_canceled; }
|
||||
|
||||
bool Command::IsInterruptible() const { return m_interruptible; }
|
||||
|
||||
void Command::SetInterruptible(bool interruptible) {
|
||||
m_interruptible = interruptible;
|
||||
}
|
||||
|
||||
bool Command::DoesRequire(Subsystem* system) const {
|
||||
return m_requirements.count(system) > 0;
|
||||
}
|
||||
|
||||
const Command::SubsystemSet& Command::GetRequirements() const {
|
||||
return m_requirements;
|
||||
}
|
||||
|
||||
CommandGroup* Command::GetGroup() const { return m_parent; }
|
||||
|
||||
void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
|
||||
|
||||
bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
|
||||
|
||||
int Command::GetID() const { return m_commandID; }
|
||||
|
||||
void Command::SetTimeout(double timeout) {
|
||||
if (timeout < 0.0)
|
||||
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
|
||||
else
|
||||
m_timeout = timeout;
|
||||
}
|
||||
|
||||
bool Command::IsTimedOut() const {
|
||||
return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
bool Command::IsParented() const { return m_parent != nullptr; }
|
||||
|
||||
void Command::ClearRequirements() { m_requirements.clear(); }
|
||||
|
||||
void Command::Initialize() {}
|
||||
|
||||
void Command::Execute() {}
|
||||
|
||||
void Command::End() {}
|
||||
|
||||
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; }
|
||||
|
||||
void Command::_Cancel() {
|
||||
if (IsRunning()) m_canceled = true;
|
||||
}
|
||||
|
||||
void Command::LockChanges() { m_locked = true; }
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void Command::StartRunning() {
|
||||
m_running = true;
|
||||
m_startTime = -1;
|
||||
m_completed = false;
|
||||
}
|
||||
|
||||
void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
|
||||
|
||||
std::string Command::GetName() const {
|
||||
return SendableRegistry::GetInstance().GetName(this);
|
||||
}
|
||||
|
||||
void Command::SetName(const wpi::Twine& name) {
|
||||
SendableRegistry::GetInstance().SetName(this, name);
|
||||
}
|
||||
|
||||
std::string Command::GetSubsystem() const {
|
||||
return SendableRegistry::GetInstance().GetSubsystem(this);
|
||||
}
|
||||
|
||||
void Command::SetSubsystem(const wpi::Twine& name) {
|
||||
SendableRegistry::GetInstance().SetSubsystem(this, name);
|
||||
}
|
||||
|
||||
void Command::InitSendable(SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Command");
|
||||
builder.AddStringProperty(
|
||||
".name", [=]() { return SendableRegistry::GetInstance().GetName(this); },
|
||||
nullptr);
|
||||
builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
|
||||
[=](bool value) {
|
||||
if (value) {
|
||||
if (!IsRunning()) Start();
|
||||
} else {
|
||||
if (IsRunning()) Cancel();
|
||||
}
|
||||
});
|
||||
builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
|
||||
nullptr);
|
||||
}
|
||||
@@ -1,243 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/CommandGroup.h"
|
||||
|
||||
#include "frc/WPIErrors.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
CommandGroup::CommandGroup(const wpi::Twine& name) : Command(name) {}
|
||||
|
||||
void CommandGroup::AddSequential(Command* command) {
|
||||
if (command == nullptr) {
|
||||
wpi_setWPIErrorWithContext(NullParameter, "command");
|
||||
return;
|
||||
}
|
||||
if (!AssertUnlocked("Cannot add new command to command group")) return;
|
||||
|
||||
m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
|
||||
|
||||
command->SetParent(this);
|
||||
|
||||
// Iterate through command->GetRequirements() and call Requires() on each
|
||||
// required subsystem
|
||||
for (auto&& requirement : command->GetRequirements()) Requires(requirement);
|
||||
}
|
||||
|
||||
void CommandGroup::AddSequential(Command* command, double timeout) {
|
||||
if (command == nullptr) {
|
||||
wpi_setWPIErrorWithContext(NullParameter, "command");
|
||||
return;
|
||||
}
|
||||
if (!AssertUnlocked("Cannot add new command to command group")) return;
|
||||
if (timeout < 0.0) {
|
||||
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
|
||||
return;
|
||||
}
|
||||
|
||||
m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
|
||||
timeout);
|
||||
|
||||
command->SetParent(this);
|
||||
|
||||
// Iterate through command->GetRequirements() and call Requires() on each
|
||||
// required subsystem
|
||||
for (auto&& requirement : command->GetRequirements()) Requires(requirement);
|
||||
}
|
||||
|
||||
void CommandGroup::AddParallel(Command* command) {
|
||||
if (command == nullptr) {
|
||||
wpi_setWPIErrorWithContext(NullParameter, "command");
|
||||
return;
|
||||
}
|
||||
if (!AssertUnlocked("Cannot add new command to command group")) return;
|
||||
|
||||
m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
|
||||
|
||||
command->SetParent(this);
|
||||
|
||||
// Iterate through command->GetRequirements() and call Requires() on each
|
||||
// required subsystem
|
||||
for (auto&& requirement : command->GetRequirements()) Requires(requirement);
|
||||
}
|
||||
|
||||
void CommandGroup::AddParallel(Command* command, double timeout) {
|
||||
if (command == nullptr) {
|
||||
wpi_setWPIErrorWithContext(NullParameter, "command");
|
||||
return;
|
||||
}
|
||||
if (!AssertUnlocked("Cannot add new command to command group")) return;
|
||||
if (timeout < 0.0) {
|
||||
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
|
||||
return;
|
||||
}
|
||||
|
||||
m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
|
||||
timeout);
|
||||
|
||||
command->SetParent(this);
|
||||
|
||||
// Iterate through command->GetRequirements() and call Requires() on each
|
||||
// required subsystem
|
||||
for (auto&& requirement : command->GetRequirements()) Requires(requirement);
|
||||
}
|
||||
|
||||
bool CommandGroup::IsInterruptible() const {
|
||||
if (!Command::IsInterruptible()) return false;
|
||||
|
||||
if (m_currentCommandIndex != -1 &&
|
||||
static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
|
||||
Command* cmd = m_commands[m_currentCommandIndex].m_command;
|
||||
if (!cmd->IsInterruptible()) return false;
|
||||
}
|
||||
|
||||
for (const auto& child : m_children) {
|
||||
if (!child->m_command->IsInterruptible()) return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int CommandGroup::GetSize() const { return m_children.size(); }
|
||||
|
||||
void CommandGroup::Initialize() {}
|
||||
|
||||
void CommandGroup::Execute() {}
|
||||
|
||||
bool CommandGroup::IsFinished() {
|
||||
return static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
|
||||
m_children.empty();
|
||||
}
|
||||
|
||||
void CommandGroup::End() {}
|
||||
|
||||
void CommandGroup::Interrupted() {}
|
||||
|
||||
void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
|
||||
|
||||
void CommandGroup::_Execute() {
|
||||
CommandGroupEntry* entry;
|
||||
Command* cmd = nullptr;
|
||||
bool firstRun = false;
|
||||
|
||||
if (m_currentCommandIndex == -1) {
|
||||
firstRun = true;
|
||||
m_currentCommandIndex = 0;
|
||||
}
|
||||
|
||||
// While there are still commands in this group to run
|
||||
while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
|
||||
// If a command is prepared to run
|
||||
if (cmd != nullptr) {
|
||||
// If command timed out, cancel it so it's removed from the Scheduler
|
||||
if (entry->IsTimedOut()) cmd->_Cancel();
|
||||
|
||||
// If command finished or was cancelled, remove it from Scheduler
|
||||
if (cmd->Run()) {
|
||||
break;
|
||||
} else {
|
||||
cmd->Removed();
|
||||
|
||||
// Advance to next command in group
|
||||
m_currentCommandIndex++;
|
||||
firstRun = true;
|
||||
cmd = nullptr;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
entry = &m_commands[m_currentCommandIndex];
|
||||
cmd = nullptr;
|
||||
|
||||
switch (entry->m_state) {
|
||||
case CommandGroupEntry::kSequence_InSequence:
|
||||
cmd = entry->m_command;
|
||||
if (firstRun) {
|
||||
cmd->StartRunning();
|
||||
CancelConflicts(cmd);
|
||||
firstRun = false;
|
||||
}
|
||||
break;
|
||||
|
||||
case CommandGroupEntry::kSequence_BranchPeer:
|
||||
// Start executing a parallel command and advance to next entry in group
|
||||
m_currentCommandIndex++;
|
||||
entry->m_command->Start();
|
||||
break;
|
||||
|
||||
case CommandGroupEntry::kSequence_BranchChild:
|
||||
m_currentCommandIndex++;
|
||||
|
||||
/* Causes scheduler to skip children of current command which require
|
||||
* the same subsystems as it
|
||||
*/
|
||||
CancelConflicts(entry->m_command);
|
||||
entry->m_command->StartRunning();
|
||||
|
||||
// Add current command entry to list of children of this group
|
||||
m_children.push_back(entry);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Run Children
|
||||
for (auto& entry : m_children) {
|
||||
auto child = entry->m_command;
|
||||
if (entry->IsTimedOut()) {
|
||||
child->_Cancel();
|
||||
}
|
||||
|
||||
// If child finished or was cancelled, set it to nullptr. nullptr entries
|
||||
// are removed later.
|
||||
if (!child->Run()) {
|
||||
child->Removed();
|
||||
entry = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
|
||||
m_children.end());
|
||||
}
|
||||
|
||||
void CommandGroup::_End() {
|
||||
// Theoretically, we don't have to check this, but we do if teams override the
|
||||
// IsFinished method
|
||||
if (m_currentCommandIndex != -1 &&
|
||||
static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
|
||||
Command* cmd = m_commands[m_currentCommandIndex].m_command;
|
||||
cmd->_Cancel();
|
||||
cmd->Removed();
|
||||
}
|
||||
|
||||
for (auto& child : m_children) {
|
||||
Command* cmd = child->m_command;
|
||||
cmd->_Cancel();
|
||||
cmd->Removed();
|
||||
}
|
||||
m_children.clear();
|
||||
}
|
||||
|
||||
void CommandGroup::_Interrupted() { _End(); }
|
||||
|
||||
void CommandGroup::CancelConflicts(Command* command) {
|
||||
for (auto childIter = m_children.begin(); childIter != m_children.end();) {
|
||||
Command* child = (*childIter)->m_command;
|
||||
bool erased = false;
|
||||
|
||||
for (auto&& requirement : command->GetRequirements()) {
|
||||
if (child->DoesRequire(requirement)) {
|
||||
child->_Cancel();
|
||||
child->Removed();
|
||||
childIter = m_children.erase(childIter);
|
||||
erased = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!erased) childIter++;
|
||||
}
|
||||
}
|
||||
@@ -1,23 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/CommandGroupEntry.h"
|
||||
|
||||
#include "frc/commands/Command.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
|
||||
double timeout)
|
||||
: m_timeout(timeout), m_command(command), m_state(state) {}
|
||||
|
||||
bool CommandGroupEntry::IsTimedOut() const {
|
||||
if (m_timeout < 0.0) return false;
|
||||
double time = m_command->TimeSinceInitialized();
|
||||
if (time == 0.0) return false;
|
||||
return time >= m_timeout;
|
||||
}
|
||||
@@ -1,80 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2017-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 "frc/commands/ConditionalCommand.h"
|
||||
|
||||
#include "frc/commands/Scheduler.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
|
||||
if (onTrue != nullptr) {
|
||||
for (auto requirement : onTrue->GetRequirements())
|
||||
command.Requires(requirement);
|
||||
}
|
||||
if (onFalse != nullptr) {
|
||||
for (auto requirement : onFalse->GetRequirements())
|
||||
command.Requires(requirement);
|
||||
}
|
||||
}
|
||||
|
||||
ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
|
||||
m_onTrue = onTrue;
|
||||
m_onFalse = onFalse;
|
||||
|
||||
RequireAll(*this, onTrue, onFalse);
|
||||
}
|
||||
|
||||
ConditionalCommand::ConditionalCommand(const wpi::Twine& name, Command* onTrue,
|
||||
Command* onFalse)
|
||||
: Command(name) {
|
||||
m_onTrue = onTrue;
|
||||
m_onFalse = onFalse;
|
||||
|
||||
RequireAll(*this, onTrue, onFalse);
|
||||
}
|
||||
|
||||
void ConditionalCommand::_Initialize() {
|
||||
if (Condition()) {
|
||||
m_chosenCommand = m_onTrue;
|
||||
} else {
|
||||
m_chosenCommand = m_onFalse;
|
||||
}
|
||||
|
||||
if (m_chosenCommand != nullptr) {
|
||||
// This is a hack to make cancelling the chosen command inside a
|
||||
// CommandGroup work properly
|
||||
m_chosenCommand->ClearRequirements();
|
||||
|
||||
m_chosenCommand->Start();
|
||||
}
|
||||
Command::_Initialize();
|
||||
}
|
||||
|
||||
void ConditionalCommand::_Cancel() {
|
||||
if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
|
||||
m_chosenCommand->Cancel();
|
||||
}
|
||||
|
||||
Command::_Cancel();
|
||||
}
|
||||
|
||||
bool ConditionalCommand::IsFinished() {
|
||||
if (m_chosenCommand != nullptr) {
|
||||
return m_chosenCommand->IsCompleted();
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void ConditionalCommand::_Interrupted() {
|
||||
if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
|
||||
m_chosenCommand->Cancel();
|
||||
}
|
||||
|
||||
Command::_Interrupted();
|
||||
}
|
||||
@@ -1,45 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2016-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 "frc/commands/InstantCommand.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
|
||||
|
||||
InstantCommand::InstantCommand(Subsystem& subsystem) : Command(subsystem) {}
|
||||
|
||||
InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem)
|
||||
: Command(name, subsystem) {}
|
||||
|
||||
InstantCommand::InstantCommand(std::function<void()> func) : m_func(func) {}
|
||||
|
||||
InstantCommand::InstantCommand(Subsystem& subsystem, std::function<void()> func)
|
||||
: InstantCommand(subsystem) {
|
||||
m_func = func;
|
||||
}
|
||||
|
||||
InstantCommand::InstantCommand(const wpi::Twine& name,
|
||||
std::function<void()> func)
|
||||
: InstantCommand(name) {
|
||||
m_func = func;
|
||||
}
|
||||
|
||||
InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
|
||||
std::function<void()> func)
|
||||
: InstantCommand(name, subsystem) {
|
||||
m_func = func;
|
||||
}
|
||||
|
||||
void InstantCommand::_Initialize() {
|
||||
Command::_Initialize();
|
||||
if (m_func) {
|
||||
m_func();
|
||||
}
|
||||
}
|
||||
|
||||
bool InstantCommand::IsFinished() { return true; }
|
||||
@@ -1,108 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/PIDCommand.h"
|
||||
|
||||
#include "frc/smartdashboard/SendableBuilder.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
|
||||
double f, double period)
|
||||
: Command(name) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
|
||||
m_controller =
|
||||
std::make_shared<PIDController>(p, i, d, f, this, this, period);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d)
|
||||
: Command(name) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
|
||||
double period)
|
||||
: Command(name) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(double p, double i, double d) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(double p, double i, double d, double period) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
|
||||
double f, double period, Subsystem& subsystem)
|
||||
: Command(name, subsystem) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(double p, double i, double d, double f, double period,
|
||||
Subsystem& subsystem)
|
||||
: Command(subsystem) {
|
||||
m_controller =
|
||||
std::make_shared<PIDController>(p, i, d, f, this, this, period);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
|
||||
Subsystem& subsystem)
|
||||
: Command(name, subsystem) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
|
||||
double period, Subsystem& subsystem)
|
||||
: Command(name, subsystem) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(double p, double i, double d, Subsystem& subsystem) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
|
||||
}
|
||||
|
||||
PIDCommand::PIDCommand(double p, double i, double d, double period,
|
||||
Subsystem& subsystem) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
|
||||
}
|
||||
|
||||
void PIDCommand::_Initialize() { m_controller->Enable(); }
|
||||
|
||||
void PIDCommand::_End() { m_controller->Disable(); }
|
||||
|
||||
void PIDCommand::_Interrupted() { _End(); }
|
||||
|
||||
void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
|
||||
SetSetpoint(GetSetpoint() + deltaSetpoint);
|
||||
}
|
||||
|
||||
void PIDCommand::PIDWrite(double output) { UsePIDOutput(output); }
|
||||
|
||||
double PIDCommand::PIDGet() { return ReturnPIDInput(); }
|
||||
|
||||
std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
|
||||
return m_controller;
|
||||
}
|
||||
|
||||
void PIDCommand::SetSetpoint(double setpoint) {
|
||||
m_controller->SetSetpoint(setpoint);
|
||||
}
|
||||
|
||||
double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
|
||||
|
||||
double PIDCommand::GetPosition() { return ReturnPIDInput(); }
|
||||
|
||||
void PIDCommand::InitSendable(SendableBuilder& builder) {
|
||||
m_controller->InitSendable(builder);
|
||||
Command::InitSendable(builder);
|
||||
builder.SetSmartDashboardType("PIDCommand");
|
||||
}
|
||||
@@ -1,97 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/PIDSubsystem.h"
|
||||
|
||||
#include "frc/PIDController.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d)
|
||||
: Subsystem(name) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
|
||||
AddChild("PIDController", m_controller);
|
||||
}
|
||||
|
||||
PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
|
||||
double f)
|
||||
: Subsystem(name) {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
|
||||
AddChild("PIDController", m_controller);
|
||||
}
|
||||
|
||||
PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
|
||||
double f, double period)
|
||||
: Subsystem(name) {
|
||||
m_controller =
|
||||
std::make_shared<PIDController>(p, i, d, f, this, this, period);
|
||||
AddChild("PIDController", m_controller);
|
||||
}
|
||||
|
||||
PIDSubsystem::PIDSubsystem(double p, double i, double d)
|
||||
: Subsystem("PIDSubsystem") {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, this, this);
|
||||
AddChild("PIDController", m_controller);
|
||||
}
|
||||
|
||||
PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
|
||||
: Subsystem("PIDSubsystem") {
|
||||
m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
|
||||
AddChild("PIDController", m_controller);
|
||||
}
|
||||
|
||||
PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
|
||||
double period)
|
||||
: Subsystem("PIDSubsystem") {
|
||||
m_controller =
|
||||
std::make_shared<PIDController>(p, i, d, f, this, this, period);
|
||||
AddChild("PIDController", m_controller);
|
||||
}
|
||||
|
||||
void PIDSubsystem::Enable() { m_controller->Enable(); }
|
||||
|
||||
void PIDSubsystem::Disable() { m_controller->Disable(); }
|
||||
|
||||
void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
|
||||
|
||||
double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
|
||||
|
||||
void PIDSubsystem::SetSetpoint(double setpoint) {
|
||||
m_controller->SetSetpoint(setpoint);
|
||||
}
|
||||
|
||||
void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
|
||||
SetSetpoint(GetSetpoint() + deltaSetpoint);
|
||||
}
|
||||
|
||||
void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
|
||||
m_controller->SetInputRange(minimumInput, maximumInput);
|
||||
}
|
||||
|
||||
void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
|
||||
m_controller->SetOutputRange(minimumOutput, maximumOutput);
|
||||
}
|
||||
|
||||
double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
|
||||
|
||||
double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
|
||||
|
||||
double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
|
||||
|
||||
void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
|
||||
m_controller->SetAbsoluteTolerance(absValue);
|
||||
}
|
||||
|
||||
void PIDSubsystem::SetPercentTolerance(double percent) {
|
||||
m_controller->SetPercentTolerance(percent);
|
||||
}
|
||||
|
||||
bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
|
||||
|
||||
std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
|
||||
return m_controller;
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/PrintCommand.h"
|
||||
|
||||
#include <wpi/raw_ostream.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
PrintCommand::PrintCommand(const wpi::Twine& message)
|
||||
: InstantCommand("Print \"" + message + wpi::Twine('"')) {
|
||||
m_message = message.str();
|
||||
}
|
||||
|
||||
void PrintCommand::Initialize() { wpi::outs() << m_message << '\n'; }
|
||||
@@ -1,245 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2011-2019 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 "frc/commands/Scheduler.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <hal/HAL.h>
|
||||
#include <networktables/NetworkTableEntry.h>
|
||||
#include <wpi/mutex.h>
|
||||
|
||||
#include "frc/WPIErrors.h"
|
||||
#include "frc/buttons/ButtonScheduler.h"
|
||||
#include "frc/commands/Command.h"
|
||||
#include "frc/commands/Subsystem.h"
|
||||
#include "frc/smartdashboard/SendableBuilder.h"
|
||||
#include "frc/smartdashboard/SendableRegistry.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
struct Scheduler::Impl {
|
||||
void Remove(Command* command);
|
||||
void ProcessCommandAddition(Command* command);
|
||||
|
||||
using SubsystemSet = std::set<Subsystem*>;
|
||||
SubsystemSet subsystems;
|
||||
wpi::mutex buttonsMutex;
|
||||
using ButtonVector = std::vector<std::unique_ptr<ButtonScheduler>>;
|
||||
ButtonVector buttons;
|
||||
using CommandVector = std::vector<Command*>;
|
||||
wpi::mutex additionsMutex;
|
||||
CommandVector additions;
|
||||
using CommandSet = std::set<Command*>;
|
||||
CommandSet commands;
|
||||
bool adding = false;
|
||||
bool enabled = true;
|
||||
std::vector<std::string> commandsBuf;
|
||||
std::vector<double> idsBuf;
|
||||
bool runningCommandsChanged = false;
|
||||
};
|
||||
|
||||
Scheduler* Scheduler::GetInstance() {
|
||||
static Scheduler instance;
|
||||
return &instance;
|
||||
}
|
||||
|
||||
void Scheduler::AddCommand(Command* command) {
|
||||
std::scoped_lock lock(m_impl->additionsMutex);
|
||||
if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
|
||||
m_impl->additions.end())
|
||||
return;
|
||||
m_impl->additions.push_back(command);
|
||||
}
|
||||
|
||||
void Scheduler::AddButton(ButtonScheduler* button) {
|
||||
std::scoped_lock lock(m_impl->buttonsMutex);
|
||||
m_impl->buttons.emplace_back(button);
|
||||
}
|
||||
|
||||
void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
|
||||
if (subsystem == nullptr) {
|
||||
wpi_setWPIErrorWithContext(NullParameter, "subsystem");
|
||||
return;
|
||||
}
|
||||
m_impl->subsystems.insert(subsystem);
|
||||
}
|
||||
|
||||
void Scheduler::Run() {
|
||||
// Get button input (going backwards preserves button priority)
|
||||
{
|
||||
if (!m_impl->enabled) return;
|
||||
|
||||
std::scoped_lock lock(m_impl->buttonsMutex);
|
||||
for (auto& button : m_impl->buttons) {
|
||||
button->Execute();
|
||||
}
|
||||
}
|
||||
|
||||
// Call every subsystem's periodic method
|
||||
for (auto& subsystem : m_impl->subsystems) {
|
||||
subsystem->Periodic();
|
||||
}
|
||||
|
||||
m_impl->runningCommandsChanged = false;
|
||||
|
||||
// Loop through the commands
|
||||
for (auto cmdIter = m_impl->commands.begin();
|
||||
cmdIter != m_impl->commands.end();) {
|
||||
Command* command = *cmdIter;
|
||||
// Increment before potentially removing to keep the iterator valid
|
||||
++cmdIter;
|
||||
if (!command->Run()) {
|
||||
Remove(command);
|
||||
m_impl->runningCommandsChanged = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Add the new things
|
||||
{
|
||||
std::scoped_lock lock(m_impl->additionsMutex);
|
||||
for (auto& addition : m_impl->additions) {
|
||||
// Check to make sure no adding during adding
|
||||
if (m_impl->adding) {
|
||||
wpi_setWPIErrorWithContext(IncompatibleState,
|
||||
"Can not start command from cancel method");
|
||||
} else {
|
||||
m_impl->ProcessCommandAddition(addition);
|
||||
}
|
||||
}
|
||||
m_impl->additions.clear();
|
||||
}
|
||||
|
||||
// Add in the defaults
|
||||
for (auto& subsystem : m_impl->subsystems) {
|
||||
if (subsystem->GetCurrentCommand() == nullptr) {
|
||||
if (m_impl->adding) {
|
||||
wpi_setWPIErrorWithContext(IncompatibleState,
|
||||
"Can not start command from cancel method");
|
||||
} else {
|
||||
m_impl->ProcessCommandAddition(subsystem->GetDefaultCommand());
|
||||
}
|
||||
}
|
||||
subsystem->ConfirmCommand();
|
||||
}
|
||||
}
|
||||
|
||||
void Scheduler::Remove(Command* command) {
|
||||
if (command == nullptr) {
|
||||
wpi_setWPIErrorWithContext(NullParameter, "command");
|
||||
return;
|
||||
}
|
||||
|
||||
m_impl->Remove(command);
|
||||
}
|
||||
|
||||
void Scheduler::RemoveAll() {
|
||||
while (m_impl->commands.size() > 0) {
|
||||
Remove(*m_impl->commands.begin());
|
||||
}
|
||||
}
|
||||
|
||||
void Scheduler::ResetAll() {
|
||||
RemoveAll();
|
||||
m_impl->subsystems.clear();
|
||||
m_impl->buttons.clear();
|
||||
m_impl->additions.clear();
|
||||
m_impl->commands.clear();
|
||||
}
|
||||
|
||||
void Scheduler::SetEnabled(bool enabled) { m_impl->enabled = enabled; }
|
||||
|
||||
void Scheduler::InitSendable(SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Scheduler");
|
||||
auto namesEntry = builder.GetEntry("Names");
|
||||
auto idsEntry = builder.GetEntry("Ids");
|
||||
auto cancelEntry = builder.GetEntry("Cancel");
|
||||
builder.SetUpdateTable([=]() {
|
||||
// Get the list of possible commands to cancel
|
||||
auto new_toCancel = cancelEntry.GetValue();
|
||||
wpi::ArrayRef<double> toCancel;
|
||||
if (new_toCancel) toCancel = new_toCancel->GetDoubleArray();
|
||||
|
||||
// Cancel commands whose cancel buttons were pressed on the SmartDashboard
|
||||
if (!toCancel.empty()) {
|
||||
for (auto& command : m_impl->commands) {
|
||||
for (const auto& cancelled : toCancel) {
|
||||
if (command->GetID() == cancelled) {
|
||||
command->Cancel();
|
||||
}
|
||||
}
|
||||
}
|
||||
nt::NetworkTableEntry(cancelEntry).SetDoubleArray({});
|
||||
}
|
||||
|
||||
// Set the running commands
|
||||
if (m_impl->runningCommandsChanged) {
|
||||
m_impl->commandsBuf.resize(0);
|
||||
m_impl->idsBuf.resize(0);
|
||||
auto& registry = SendableRegistry::GetInstance();
|
||||
for (const auto& command : m_impl->commands) {
|
||||
m_impl->commandsBuf.emplace_back(registry.GetName(command));
|
||||
m_impl->idsBuf.emplace_back(command->GetID());
|
||||
}
|
||||
nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
|
||||
nt::NetworkTableEntry(idsEntry).SetDoubleArray(m_impl->idsBuf);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
Scheduler::Scheduler() : m_impl(new Impl) {
|
||||
HAL_Report(HALUsageReporting::kResourceType_Command,
|
||||
HALUsageReporting::kCommand_Scheduler);
|
||||
SendableRegistry::GetInstance().AddLW(this, "Scheduler");
|
||||
}
|
||||
|
||||
Scheduler::~Scheduler() {}
|
||||
|
||||
void Scheduler::Impl::Remove(Command* command) {
|
||||
if (!commands.erase(command)) return;
|
||||
|
||||
for (auto&& requirement : command->GetRequirements()) {
|
||||
requirement->SetCurrentCommand(nullptr);
|
||||
}
|
||||
|
||||
command->Removed();
|
||||
}
|
||||
|
||||
void Scheduler::Impl::ProcessCommandAddition(Command* command) {
|
||||
if (command == nullptr) return;
|
||||
|
||||
// Only add if not already in
|
||||
auto found = commands.find(command);
|
||||
if (found == commands.end()) {
|
||||
// Check that the requirements can be had
|
||||
const auto& requirements = command->GetRequirements();
|
||||
for (const auto& requirement : requirements) {
|
||||
if (requirement->GetCurrentCommand() != nullptr &&
|
||||
!requirement->GetCurrentCommand()->IsInterruptible())
|
||||
return;
|
||||
}
|
||||
|
||||
// Give it the requirements
|
||||
adding = true;
|
||||
for (auto&& requirement : requirements) {
|
||||
if (requirement->GetCurrentCommand() != nullptr) {
|
||||
requirement->GetCurrentCommand()->Cancel();
|
||||
Remove(requirement->GetCurrentCommand());
|
||||
}
|
||||
requirement->SetCurrentCommand(command);
|
||||
}
|
||||
adding = false;
|
||||
|
||||
commands.insert(command);
|
||||
|
||||
command->StartRunning();
|
||||
runningCommandsChanged = true;
|
||||
}
|
||||
}
|
||||
@@ -1,17 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/StartCommand.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
StartCommand::StartCommand(Command* commandToStart)
|
||||
: InstantCommand("StartCommand") {
|
||||
m_commandToFork = commandToStart;
|
||||
}
|
||||
|
||||
void StartCommand::Initialize() { m_commandToFork->Start(); }
|
||||
@@ -1,134 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2011-2019 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 "frc/commands/Subsystem.h"
|
||||
|
||||
#include "frc/WPIErrors.h"
|
||||
#include "frc/commands/Command.h"
|
||||
#include "frc/commands/Scheduler.h"
|
||||
#include "frc/livewindow/LiveWindow.h"
|
||||
#include "frc/smartdashboard/SendableBuilder.h"
|
||||
#include "frc/smartdashboard/SendableRegistry.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Subsystem::Subsystem(const wpi::Twine& name) {
|
||||
SendableRegistry::GetInstance().AddLW(this, name, name);
|
||||
Scheduler::GetInstance()->RegisterSubsystem(this);
|
||||
}
|
||||
|
||||
void Subsystem::SetDefaultCommand(Command* command) {
|
||||
if (command == nullptr) {
|
||||
m_defaultCommand = nullptr;
|
||||
} else {
|
||||
const auto& reqs = command->GetRequirements();
|
||||
if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
|
||||
wpi_setWPIErrorWithContext(
|
||||
CommandIllegalUse, "A default command must require the subsystem");
|
||||
return;
|
||||
}
|
||||
|
||||
m_defaultCommand = command;
|
||||
}
|
||||
}
|
||||
|
||||
Command* Subsystem::GetDefaultCommand() {
|
||||
if (!m_initializedDefaultCommand) {
|
||||
m_initializedDefaultCommand = true;
|
||||
InitDefaultCommand();
|
||||
}
|
||||
return m_defaultCommand;
|
||||
}
|
||||
|
||||
wpi::StringRef Subsystem::GetDefaultCommandName() {
|
||||
Command* defaultCommand = GetDefaultCommand();
|
||||
if (defaultCommand) {
|
||||
return SendableRegistry::GetInstance().GetName(defaultCommand);
|
||||
} else {
|
||||
return wpi::StringRef();
|
||||
}
|
||||
}
|
||||
|
||||
void Subsystem::SetCurrentCommand(Command* command) {
|
||||
m_currentCommand = command;
|
||||
m_currentCommandChanged = true;
|
||||
}
|
||||
|
||||
Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
|
||||
|
||||
wpi::StringRef Subsystem::GetCurrentCommandName() const {
|
||||
Command* currentCommand = GetCurrentCommand();
|
||||
if (currentCommand) {
|
||||
return SendableRegistry::GetInstance().GetName(currentCommand);
|
||||
} else {
|
||||
return wpi::StringRef();
|
||||
}
|
||||
}
|
||||
|
||||
void Subsystem::Periodic() {}
|
||||
|
||||
void Subsystem::InitDefaultCommand() {}
|
||||
|
||||
std::string Subsystem::GetName() const {
|
||||
return SendableRegistry::GetInstance().GetName(this);
|
||||
}
|
||||
|
||||
void Subsystem::SetName(const wpi::Twine& name) {
|
||||
SendableRegistry::GetInstance().SetName(this, name);
|
||||
}
|
||||
|
||||
std::string Subsystem::GetSubsystem() const {
|
||||
return SendableRegistry::GetInstance().GetSubsystem(this);
|
||||
}
|
||||
|
||||
void Subsystem::SetSubsystem(const wpi::Twine& name) {
|
||||
SendableRegistry::GetInstance().SetSubsystem(this, name);
|
||||
}
|
||||
|
||||
void Subsystem::AddChild(const wpi::Twine& name,
|
||||
std::shared_ptr<Sendable> child) {
|
||||
AddChild(name, *child);
|
||||
}
|
||||
|
||||
void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
|
||||
AddChild(name, *child);
|
||||
}
|
||||
|
||||
void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
|
||||
auto& registry = SendableRegistry::GetInstance();
|
||||
registry.AddLW(&child, registry.GetSubsystem(this), name);
|
||||
registry.AddChild(this, &child);
|
||||
}
|
||||
|
||||
void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
|
||||
|
||||
void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
|
||||
|
||||
void Subsystem::AddChild(Sendable& child) {
|
||||
auto& registry = SendableRegistry::GetInstance();
|
||||
registry.SetSubsystem(&child, registry.GetSubsystem(this));
|
||||
registry.EnableLiveWindow(&child);
|
||||
registry.AddChild(this, &child);
|
||||
}
|
||||
|
||||
void Subsystem::ConfirmCommand() {
|
||||
if (m_currentCommandChanged) m_currentCommandChanged = false;
|
||||
}
|
||||
|
||||
void Subsystem::InitSendable(SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Subsystem");
|
||||
|
||||
builder.AddBooleanProperty(
|
||||
".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
|
||||
builder.AddStringProperty(".default",
|
||||
[=]() { return GetDefaultCommandName(); }, nullptr);
|
||||
|
||||
builder.AddBooleanProperty(
|
||||
".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
|
||||
builder.AddStringProperty(".command",
|
||||
[=]() { return GetCurrentCommandName(); }, nullptr);
|
||||
}
|
||||
@@ -1,24 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2016-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 "frc/commands/TimedCommand.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
|
||||
: Command(name, timeout) {}
|
||||
|
||||
TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
|
||||
|
||||
TimedCommand::TimedCommand(const wpi::Twine& name, double timeout,
|
||||
Subsystem& subsystem)
|
||||
: Command(name, timeout, subsystem) {}
|
||||
|
||||
TimedCommand::TimedCommand(double timeout, Subsystem& subsystem)
|
||||
: Command(timeout, subsystem) {}
|
||||
|
||||
bool TimedCommand::IsFinished() { return IsTimedOut(); }
|
||||
@@ -1,16 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/WaitCommand.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
WaitCommand::WaitCommand(double timeout)
|
||||
: TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
|
||||
|
||||
WaitCommand::WaitCommand(const wpi::Twine& name, double timeout)
|
||||
: TimedCommand(name, timeout) {}
|
||||
@@ -1,22 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/WaitForChildren.h"
|
||||
|
||||
#include "frc/commands/CommandGroup.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
WaitForChildren::WaitForChildren(double timeout)
|
||||
: Command("WaitForChildren", timeout) {}
|
||||
|
||||
WaitForChildren::WaitForChildren(const wpi::Twine& name, double timeout)
|
||||
: Command(name, timeout) {}
|
||||
|
||||
bool WaitForChildren::IsFinished() {
|
||||
return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
|
||||
}
|
||||
@@ -1,24 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 "frc/commands/WaitUntilCommand.h"
|
||||
|
||||
#include "frc/Timer.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
WaitUntilCommand::WaitUntilCommand(double time)
|
||||
: Command("WaitUntilCommand", time) {
|
||||
m_time = time;
|
||||
}
|
||||
|
||||
WaitUntilCommand::WaitUntilCommand(const wpi::Twine& name, double time)
|
||||
: Command(name, time) {
|
||||
m_time = time;
|
||||
}
|
||||
|
||||
bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }
|
||||
@@ -1,108 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/Command.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "frc2/command/CommandScheduler.h"
|
||||
#include "frc2/command/InstantCommand.h"
|
||||
#include "frc2/command/ParallelCommandGroup.h"
|
||||
#include "frc2/command/ParallelDeadlineGroup.h"
|
||||
#include "frc2/command/ParallelRaceGroup.h"
|
||||
#include "frc2/command/PerpetualCommand.h"
|
||||
#include "frc2/command/ProxyScheduleCommand.h"
|
||||
#include "frc2/command/SequentialCommandGroup.h"
|
||||
#include "frc2/command/WaitCommand.h"
|
||||
#include "frc2/command/WaitUntilCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
Command::~Command() { CommandScheduler::GetInstance().Cancel(this); }
|
||||
|
||||
Command::Command(const Command& rhs) : ErrorBase(rhs) {}
|
||||
|
||||
Command& Command::operator=(const Command& rhs) {
|
||||
ErrorBase::operator=(rhs);
|
||||
m_isGrouped = false;
|
||||
return *this;
|
||||
}
|
||||
|
||||
void Command::Initialize() {}
|
||||
void Command::Execute() {}
|
||||
void Command::End(bool interrupted) {}
|
||||
|
||||
ParallelRaceGroup Command::WithTimeout(units::second_t duration) && {
|
||||
std::vector<std::unique_ptr<Command>> temp;
|
||||
temp.emplace_back(std::make_unique<WaitCommand>(duration));
|
||||
temp.emplace_back(std::move(*this).TransferOwnership());
|
||||
return ParallelRaceGroup(std::move(temp));
|
||||
}
|
||||
|
||||
ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
|
||||
std::vector<std::unique_ptr<Command>> temp;
|
||||
temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
|
||||
temp.emplace_back(std::move(*this).TransferOwnership());
|
||||
return ParallelRaceGroup(std::move(temp));
|
||||
}
|
||||
|
||||
SequentialCommandGroup Command::BeforeStarting(std::function<void()> toRun) && {
|
||||
std::vector<std::unique_ptr<Command>> temp;
|
||||
temp.emplace_back(std::make_unique<InstantCommand>(
|
||||
std::move(toRun), std::initializer_list<Subsystem*>{}));
|
||||
temp.emplace_back(std::move(*this).TransferOwnership());
|
||||
return SequentialCommandGroup(std::move(temp));
|
||||
}
|
||||
|
||||
SequentialCommandGroup Command::AndThen(std::function<void()> toRun) && {
|
||||
std::vector<std::unique_ptr<Command>> temp;
|
||||
temp.emplace_back(std::move(*this).TransferOwnership());
|
||||
temp.emplace_back(std::make_unique<InstantCommand>(
|
||||
std::move(toRun), std::initializer_list<Subsystem*>{}));
|
||||
return SequentialCommandGroup(std::move(temp));
|
||||
}
|
||||
|
||||
PerpetualCommand Command::Perpetually() && {
|
||||
return PerpetualCommand(std::move(*this).TransferOwnership());
|
||||
}
|
||||
|
||||
ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); }
|
||||
|
||||
void Command::Schedule(bool interruptible) {
|
||||
CommandScheduler::GetInstance().Schedule(interruptible, this);
|
||||
}
|
||||
|
||||
void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); }
|
||||
|
||||
bool Command::IsScheduled() const {
|
||||
return CommandScheduler::GetInstance().IsScheduled(this);
|
||||
}
|
||||
|
||||
bool Command::HasRequirement(Subsystem* requirement) const {
|
||||
bool hasRequirement = false;
|
||||
for (auto&& subsystem : GetRequirements()) {
|
||||
hasRequirement |= requirement == subsystem;
|
||||
}
|
||||
return hasRequirement;
|
||||
}
|
||||
|
||||
std::string Command::GetName() const { return GetTypeName(*this); }
|
||||
|
||||
bool Command::IsGrouped() const { return m_isGrouped; }
|
||||
|
||||
void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; }
|
||||
|
||||
namespace frc2 {
|
||||
bool RequirementsDisjoint(Command* first, Command* second) {
|
||||
bool disjoint = true;
|
||||
auto&& requirements = second->GetRequirements();
|
||||
for (auto&& requirement : first->GetRequirements()) {
|
||||
disjoint &= requirements.find(requirement) == requirements.end();
|
||||
}
|
||||
return disjoint;
|
||||
}
|
||||
} // namespace frc2
|
||||
@@ -1,62 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/CommandBase.h"
|
||||
|
||||
#include <frc/smartdashboard/SendableBuilder.h>
|
||||
#include <frc/smartdashboard/SendableRegistry.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
#include <frc2/command/SetUtilities.h>
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
CommandBase::CommandBase() {
|
||||
frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
|
||||
}
|
||||
|
||||
void CommandBase::AddRequirements(
|
||||
std::initializer_list<Subsystem*> requirements) {
|
||||
m_requirements.insert(requirements.begin(), requirements.end());
|
||||
}
|
||||
|
||||
void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
|
||||
m_requirements.insert(requirements.begin(), requirements.end());
|
||||
}
|
||||
|
||||
wpi::SmallSet<Subsystem*, 4> CommandBase::GetRequirements() const {
|
||||
return m_requirements;
|
||||
}
|
||||
|
||||
void CommandBase::SetName(const wpi::Twine& name) {
|
||||
frc::SendableRegistry::GetInstance().SetName(this, name);
|
||||
}
|
||||
|
||||
std::string CommandBase::GetName() const {
|
||||
return frc::SendableRegistry::GetInstance().GetName(this);
|
||||
}
|
||||
|
||||
std::string CommandBase::GetSubsystem() const {
|
||||
return frc::SendableRegistry::GetInstance().GetSubsystem(this);
|
||||
}
|
||||
|
||||
void CommandBase::SetSubsystem(const wpi::Twine& subsystem) {
|
||||
frc::SendableRegistry::GetInstance().SetSubsystem(this, subsystem);
|
||||
}
|
||||
|
||||
void CommandBase::InitSendable(frc::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Command");
|
||||
builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
|
||||
builder.AddBooleanProperty("running", [this] { return IsScheduled(); },
|
||||
[this](bool value) {
|
||||
bool isScheduled = IsScheduled();
|
||||
if (value && !isScheduled) {
|
||||
Schedule();
|
||||
} else if (!value && isScheduled) {
|
||||
Cancel();
|
||||
}
|
||||
});
|
||||
}
|
||||
@@ -1,60 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/CommandGroupBase.h"
|
||||
|
||||
#include <set>
|
||||
|
||||
#include "frc/WPIErrors.h"
|
||||
#include "frc2/command/ParallelCommandGroup.h"
|
||||
#include "frc2/command/ParallelDeadlineGroup.h"
|
||||
#include "frc2/command/ParallelRaceGroup.h"
|
||||
#include "frc2/command/SequentialCommandGroup.h"
|
||||
|
||||
using namespace frc2;
|
||||
template <typename TMap, typename TKey>
|
||||
static bool ContainsKey(const TMap& map, TKey keyToCheck) {
|
||||
return map.find(keyToCheck) != map.end();
|
||||
}
|
||||
bool CommandGroupBase::RequireUngrouped(Command& command) {
|
||||
if (command.IsGrouped()) {
|
||||
wpi_setGlobalWPIErrorWithContext(
|
||||
CommandIllegalUse,
|
||||
"Commands cannot be added to more than one CommandGroup");
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool CommandGroupBase::RequireUngrouped(
|
||||
wpi::ArrayRef<std::unique_ptr<Command>> commands) {
|
||||
bool allUngrouped = true;
|
||||
for (auto&& command : commands) {
|
||||
allUngrouped &= !command.get()->IsGrouped();
|
||||
}
|
||||
if (!allUngrouped) {
|
||||
wpi_setGlobalWPIErrorWithContext(
|
||||
CommandIllegalUse,
|
||||
"Commands cannot be added to more than one CommandGroup");
|
||||
}
|
||||
return allUngrouped;
|
||||
}
|
||||
|
||||
bool CommandGroupBase::RequireUngrouped(
|
||||
std::initializer_list<Command*> commands) {
|
||||
bool allUngrouped = true;
|
||||
for (auto&& command : commands) {
|
||||
allUngrouped &= !command->IsGrouped();
|
||||
}
|
||||
if (!allUngrouped) {
|
||||
wpi_setGlobalWPIErrorWithContext(
|
||||
CommandIllegalUse,
|
||||
"Commands cannot be added to more than one CommandGroup");
|
||||
}
|
||||
return allUngrouped;
|
||||
}
|
||||
@@ -1,346 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/CommandScheduler.h"
|
||||
|
||||
#include <frc/RobotState.h>
|
||||
#include <frc/WPIErrors.h>
|
||||
#include <frc/commands/Scheduler.h>
|
||||
#include <frc/smartdashboard/SendableBuilder.h>
|
||||
#include <frc/smartdashboard/SendableRegistry.h>
|
||||
#include <frc2/command/CommandGroupBase.h>
|
||||
#include <frc2/command/Subsystem.h>
|
||||
|
||||
#include <hal/HAL.h>
|
||||
|
||||
using namespace frc2;
|
||||
template <typename TMap, typename TKey>
|
||||
static bool ContainsKey(const TMap& map, TKey keyToCheck) {
|
||||
return map.find(keyToCheck) != map.end();
|
||||
}
|
||||
|
||||
CommandScheduler::CommandScheduler() {
|
||||
frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
|
||||
}
|
||||
|
||||
CommandScheduler& CommandScheduler::GetInstance() {
|
||||
static CommandScheduler scheduler;
|
||||
return scheduler;
|
||||
}
|
||||
|
||||
void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
|
||||
m_buttons.emplace_back(std::move(button));
|
||||
}
|
||||
|
||||
void CommandScheduler::ClearButtons() { m_buttons.clear(); }
|
||||
|
||||
void CommandScheduler::Schedule(bool interruptible, Command* command) {
|
||||
if (m_inRunLoop) {
|
||||
m_toSchedule.try_emplace(command, interruptible);
|
||||
return;
|
||||
}
|
||||
|
||||
if (command->IsGrouped()) {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"A command that is part of a command group "
|
||||
"cannot be independently scheduled");
|
||||
return;
|
||||
}
|
||||
if (m_disabled ||
|
||||
(frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
|
||||
ContainsKey(m_scheduledCommands, command)) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto& requirements = command->GetRequirements();
|
||||
|
||||
wpi::SmallVector<Command*, 8> intersection;
|
||||
|
||||
bool isDisjoint = true;
|
||||
bool allInterruptible = true;
|
||||
for (auto&& i1 : m_requirements) {
|
||||
if (requirements.find(i1.first) != requirements.end()) {
|
||||
isDisjoint = false;
|
||||
allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible();
|
||||
intersection.emplace_back(i1.second);
|
||||
}
|
||||
}
|
||||
|
||||
if (isDisjoint || allInterruptible) {
|
||||
if (allInterruptible) {
|
||||
for (auto&& cmdToCancel : intersection) {
|
||||
Cancel(cmdToCancel);
|
||||
}
|
||||
}
|
||||
command->Initialize();
|
||||
m_scheduledCommands[command] = CommandState{interruptible};
|
||||
for (auto&& action : m_initActions) {
|
||||
action(*command);
|
||||
}
|
||||
for (auto&& requirement : requirements) {
|
||||
m_requirements[requirement] = command;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
|
||||
|
||||
void CommandScheduler::Schedule(bool interruptible,
|
||||
wpi::ArrayRef<Command*> commands) {
|
||||
for (auto command : commands) {
|
||||
Schedule(interruptible, command);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Schedule(bool interruptible,
|
||||
std::initializer_list<Command*> commands) {
|
||||
for (auto command : commands) {
|
||||
Schedule(interruptible, command);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
|
||||
for (auto command : commands) {
|
||||
Schedule(true, command);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
|
||||
for (auto command : commands) {
|
||||
Schedule(true, command);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Run() {
|
||||
if (m_disabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Run the periodic method of all registered subsystems.
|
||||
for (auto&& subsystem : m_subsystems) {
|
||||
subsystem.getFirst()->Periodic();
|
||||
}
|
||||
|
||||
// Poll buttons for new commands to add.
|
||||
for (auto&& button : m_buttons) {
|
||||
button();
|
||||
}
|
||||
|
||||
m_inRunLoop = true;
|
||||
// Run scheduled commands, remove finished commands.
|
||||
for (auto iterator = m_scheduledCommands.begin();
|
||||
iterator != m_scheduledCommands.end(); iterator++) {
|
||||
Command* command = iterator->getFirst();
|
||||
|
||||
if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
|
||||
Cancel(command);
|
||||
continue;
|
||||
}
|
||||
|
||||
command->Execute();
|
||||
for (auto&& action : m_executeActions) {
|
||||
action(*command);
|
||||
}
|
||||
|
||||
if (command->IsFinished()) {
|
||||
command->End(false);
|
||||
for (auto&& action : m_finishActions) {
|
||||
action(*command);
|
||||
}
|
||||
|
||||
for (auto&& requirement : command->GetRequirements()) {
|
||||
m_requirements.erase(requirement);
|
||||
}
|
||||
|
||||
m_scheduledCommands.erase(iterator);
|
||||
}
|
||||
}
|
||||
m_inRunLoop = false;
|
||||
|
||||
for (auto&& commandInterruptible : m_toSchedule) {
|
||||
Schedule(commandInterruptible.second, commandInterruptible.first);
|
||||
}
|
||||
|
||||
for (auto&& command : m_toCancel) {
|
||||
Cancel(command);
|
||||
}
|
||||
|
||||
m_toSchedule.clear();
|
||||
m_toCancel.clear();
|
||||
|
||||
// Add default commands for un-required registered subsystems.
|
||||
for (auto&& subsystem : m_subsystems) {
|
||||
auto s = m_requirements.find(subsystem.getFirst());
|
||||
if (s == m_requirements.end()) {
|
||||
Schedule({subsystem.getSecond().get()});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
|
||||
m_subsystems[subsystem] = nullptr;
|
||||
}
|
||||
|
||||
void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
|
||||
auto s = m_subsystems.find(subsystem);
|
||||
if (s != m_subsystems.end()) {
|
||||
m_subsystems.erase(s);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::RegisterSubsystem(
|
||||
std::initializer_list<Subsystem*> subsystems) {
|
||||
for (auto* subsystem : subsystems) {
|
||||
RegisterSubsystem(subsystem);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::UnregisterSubsystem(
|
||||
std::initializer_list<Subsystem*> subsystems) {
|
||||
for (auto* subsystem : subsystems) {
|
||||
UnregisterSubsystem(subsystem);
|
||||
}
|
||||
}
|
||||
|
||||
Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
|
||||
auto&& find = m_subsystems.find(subsystem);
|
||||
if (find != m_subsystems.end()) {
|
||||
return find->second.get();
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Cancel(Command* command) {
|
||||
if (m_inRunLoop) {
|
||||
m_toCancel.emplace_back(command);
|
||||
return;
|
||||
}
|
||||
|
||||
auto find = m_scheduledCommands.find(command);
|
||||
if (find == m_scheduledCommands.end()) return;
|
||||
command->End(true);
|
||||
for (auto&& action : m_interruptActions) {
|
||||
action(*command);
|
||||
}
|
||||
m_scheduledCommands.erase(find);
|
||||
for (auto&& requirement : m_requirements) {
|
||||
if (requirement.second == command) {
|
||||
m_requirements.erase(requirement.first);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
|
||||
for (auto command : commands) {
|
||||
Cancel(command);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
|
||||
for (auto command : commands) {
|
||||
Cancel(command);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::CancelAll() {
|
||||
for (auto&& command : m_scheduledCommands) {
|
||||
Cancel(command.first);
|
||||
}
|
||||
}
|
||||
|
||||
double CommandScheduler::TimeSinceScheduled(const Command* command) const {
|
||||
auto find = m_scheduledCommands.find(command);
|
||||
if (find != m_scheduledCommands.end()) {
|
||||
return find->second.TimeSinceInitialized();
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
bool CommandScheduler::IsScheduled(
|
||||
wpi::ArrayRef<const Command*> commands) const {
|
||||
for (auto command : commands) {
|
||||
if (!IsScheduled(command)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CommandScheduler::IsScheduled(
|
||||
std::initializer_list<const Command*> commands) const {
|
||||
for (auto command : commands) {
|
||||
if (!IsScheduled(command)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CommandScheduler::IsScheduled(const Command* command) const {
|
||||
return m_scheduledCommands.find(command) != m_scheduledCommands.end();
|
||||
}
|
||||
|
||||
Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
|
||||
auto find = m_requirements.find(subsystem);
|
||||
if (find != m_requirements.end()) {
|
||||
return find->second;
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void CommandScheduler::Disable() { m_disabled = true; }
|
||||
|
||||
void CommandScheduler::Enable() { m_disabled = false; }
|
||||
|
||||
void CommandScheduler::OnCommandInitialize(Action action) {
|
||||
m_initActions.emplace_back(std::move(action));
|
||||
}
|
||||
|
||||
void CommandScheduler::OnCommandExecute(Action action) {
|
||||
m_executeActions.emplace_back(std::move(action));
|
||||
}
|
||||
|
||||
void CommandScheduler::OnCommandInterrupt(Action action) {
|
||||
m_interruptActions.emplace_back(std::move(action));
|
||||
}
|
||||
|
||||
void CommandScheduler::OnCommandFinish(Action action) {
|
||||
m_finishActions.emplace_back(std::move(action));
|
||||
}
|
||||
|
||||
void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Scheduler");
|
||||
m_namesEntry = builder.GetEntry("Names");
|
||||
m_idsEntry = builder.GetEntry("Ids");
|
||||
m_cancelEntry = builder.GetEntry("Cancel");
|
||||
|
||||
builder.SetUpdateTable([this] {
|
||||
double tmp[1];
|
||||
tmp[0] = 0;
|
||||
auto toCancel = m_cancelEntry.GetDoubleArray(tmp);
|
||||
for (auto cancel : toCancel) {
|
||||
uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
|
||||
Command* command = reinterpret_cast<Command*>(ptrTmp);
|
||||
if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) {
|
||||
Cancel(command);
|
||||
}
|
||||
m_cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
|
||||
}
|
||||
|
||||
wpi::SmallVector<std::string, 8> names;
|
||||
wpi::SmallVector<double, 8> ids;
|
||||
for (auto&& command : m_scheduledCommands) {
|
||||
names.emplace_back(command.first->GetName());
|
||||
uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
|
||||
ids.emplace_back(static_cast<double>(ptrTmp));
|
||||
}
|
||||
m_namesEntry.SetStringArray(names);
|
||||
m_idsEntry.SetDoubleArray(ids);
|
||||
});
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/CommandState.h"
|
||||
|
||||
#include "frc/Timer.h"
|
||||
|
||||
using namespace frc2;
|
||||
CommandState::CommandState(bool interruptible)
|
||||
: m_interruptible{interruptible} {
|
||||
StartTiming();
|
||||
StartRunning();
|
||||
}
|
||||
|
||||
void CommandState::StartTiming() {
|
||||
m_startTime = frc::Timer::GetFPGATimestamp();
|
||||
}
|
||||
void CommandState::StartRunning() { m_startTime = -1; }
|
||||
double CommandState::TimeSinceInitialized() const {
|
||||
return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1;
|
||||
}
|
||||
@@ -1,52 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/ConditionalCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
ConditionalCommand::ConditionalCommand(std::unique_ptr<Command>&& onTrue,
|
||||
std::unique_ptr<Command>&& onFalse,
|
||||
std::function<bool()> condition)
|
||||
: m_condition{std::move(condition)} {
|
||||
if (!CommandGroupBase::RequireUngrouped({onTrue.get(), onFalse.get()})) {
|
||||
return;
|
||||
}
|
||||
|
||||
m_onTrue = std::move(onTrue);
|
||||
m_onFalse = std::move(onFalse);
|
||||
|
||||
m_onTrue->SetGrouped(true);
|
||||
m_onFalse->SetGrouped(true);
|
||||
|
||||
m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled();
|
||||
m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled();
|
||||
|
||||
AddRequirements(m_onTrue->GetRequirements());
|
||||
AddRequirements(m_onFalse->GetRequirements());
|
||||
}
|
||||
|
||||
void ConditionalCommand::Initialize() {
|
||||
if (m_condition()) {
|
||||
m_selectedCommand = m_onTrue.get();
|
||||
} else {
|
||||
m_selectedCommand = m_onFalse.get();
|
||||
}
|
||||
m_selectedCommand->Initialize();
|
||||
}
|
||||
|
||||
void ConditionalCommand::Execute() { m_selectedCommand->Execute(); }
|
||||
|
||||
void ConditionalCommand::End(bool interrupted) {
|
||||
m_selectedCommand->End(interrupted);
|
||||
}
|
||||
|
||||
bool ConditionalCommand::IsFinished() {
|
||||
return m_selectedCommand->IsFinished();
|
||||
}
|
||||
|
||||
bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; }
|
||||
@@ -1,27 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/FunctionalCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
|
||||
std::function<void()> onExecute,
|
||||
std::function<void(bool)> onEnd,
|
||||
std::function<bool()> isFinished)
|
||||
: m_onInit{std::move(onInit)},
|
||||
m_onExecute{std::move(onExecute)},
|
||||
m_onEnd{std::move(onEnd)},
|
||||
m_isFinished{std::move(isFinished)} {}
|
||||
|
||||
void FunctionalCommand::Initialize() { m_onInit(); }
|
||||
|
||||
void FunctionalCommand::Execute() { m_onExecute(); }
|
||||
|
||||
void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); }
|
||||
|
||||
bool FunctionalCommand::IsFinished() { return m_isFinished(); }
|
||||
@@ -1,22 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/InstantCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
InstantCommand::InstantCommand(std::function<void()> toRun,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_toRun{std::move(toRun)} {
|
||||
AddRequirements(requirements);
|
||||
}
|
||||
|
||||
InstantCommand::InstantCommand() : m_toRun{[] {}} {}
|
||||
|
||||
void InstantCommand::Initialize() { m_toRun(); }
|
||||
|
||||
bool InstantCommand::IsFinished() { return true; }
|
||||
@@ -1,33 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/NotifierCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
NotifierCommand::NotifierCommand(std::function<void()> toRun,
|
||||
units::second_t period,
|
||||
std::initializer_list<Subsystem*> 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),
|
||||
m_notifier(other.m_toRun),
|
||||
m_period(other.m_period) {}
|
||||
|
||||
NotifierCommand::NotifierCommand(const NotifierCommand& other)
|
||||
: CommandHelper(other),
|
||||
m_toRun(other.m_toRun),
|
||||
m_notifier(frc::Notifier(other.m_toRun)),
|
||||
m_period(other.m_period) {}
|
||||
|
||||
void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); }
|
||||
|
||||
void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); }
|
||||
@@ -1,39 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/PIDCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
PIDCommand::PIDCommand(PIDController controller,
|
||||
std::function<double()> measurementSource,
|
||||
std::function<double()> setpointSource,
|
||||
std::function<void(double)> useOutput,
|
||||
std::initializer_list<Subsystem*> 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<double()> measurementSource,
|
||||
double setpoint, std::function<void(double)> useOutput,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
|
||||
useOutput, requirements) {}
|
||||
|
||||
void PIDCommand::Initialize() { m_controller.Reset(); }
|
||||
|
||||
void PIDCommand::Execute() {
|
||||
m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint()));
|
||||
}
|
||||
|
||||
void PIDCommand::End(bool interrupted) { m_useOutput(0); }
|
||||
|
||||
PIDController& PIDCommand::getController() { return m_controller; }
|
||||
@@ -1,31 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/PIDSubsystem.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
PIDSubsystem::PIDSubsystem(PIDController controller)
|
||||
: m_controller{controller} {}
|
||||
|
||||
void PIDSubsystem::Periodic() {
|
||||
if (m_enabled) {
|
||||
UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint()));
|
||||
}
|
||||
}
|
||||
|
||||
void PIDSubsystem::Enable() {
|
||||
m_controller.Reset();
|
||||
m_enabled = true;
|
||||
}
|
||||
|
||||
void PIDSubsystem::Disable() {
|
||||
UseOutput(0);
|
||||
m_enabled = false;
|
||||
}
|
||||
|
||||
PIDController& PIDSubsystem::GetController() { return m_controller; }
|
||||
@@ -1,83 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/ParallelCommandGroup.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
ParallelCommandGroup::ParallelCommandGroup(
|
||||
std::vector<std::unique_ptr<Command>>&& commands) {
|
||||
AddCommands(std::move(commands));
|
||||
}
|
||||
|
||||
void ParallelCommandGroup::Initialize() {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
commandRunning.first->Initialize();
|
||||
commandRunning.second = true;
|
||||
}
|
||||
isRunning = true;
|
||||
}
|
||||
|
||||
void ParallelCommandGroup::Execute() {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
if (!commandRunning.second) continue;
|
||||
commandRunning.first->Execute();
|
||||
if (commandRunning.first->IsFinished()) {
|
||||
commandRunning.first->End(false);
|
||||
commandRunning.second = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ParallelCommandGroup::End(bool interrupted) {
|
||||
if (interrupted) {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
if (commandRunning.second) {
|
||||
commandRunning.first->End(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
isRunning = false;
|
||||
}
|
||||
|
||||
bool ParallelCommandGroup::IsFinished() {
|
||||
for (auto& command : m_commands) {
|
||||
if (command.second) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ParallelCommandGroup::RunsWhenDisabled() const {
|
||||
return m_runWhenDisabled;
|
||||
}
|
||||
|
||||
void ParallelCommandGroup::AddCommands(
|
||||
std::vector<std::unique_ptr<Command>>&& commands) {
|
||||
for (auto&& command : commands) {
|
||||
if (!RequireUngrouped(*command)) return;
|
||||
}
|
||||
|
||||
if (isRunning) {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"Commands cannot be added to a CommandGroup "
|
||||
"while the group is running");
|
||||
}
|
||||
|
||||
for (auto&& command : commands) {
|
||||
if (RequirementsDisjoint(this, command.get())) {
|
||||
command->SetGrouped(true);
|
||||
AddRequirements(command->GetRequirements());
|
||||
m_runWhenDisabled &= command->RunsWhenDisabled();
|
||||
m_commands[std::move(command)] = false;
|
||||
} else {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"Multiple commands in a parallel group cannot "
|
||||
"require the same subsystems");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,86 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/ParallelDeadlineGroup.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
ParallelDeadlineGroup::ParallelDeadlineGroup(
|
||||
std::unique_ptr<Command>&& deadline,
|
||||
std::vector<std::unique_ptr<Command>>&& commands) {
|
||||
SetDeadline(std::move(deadline));
|
||||
AddCommands(std::move(commands));
|
||||
}
|
||||
|
||||
void ParallelDeadlineGroup::Initialize() {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
commandRunning.first->Initialize();
|
||||
commandRunning.second = true;
|
||||
}
|
||||
isRunning = true;
|
||||
}
|
||||
|
||||
void ParallelDeadlineGroup::Execute() {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
if (!commandRunning.second) continue;
|
||||
commandRunning.first->Execute();
|
||||
if (commandRunning.first->IsFinished()) {
|
||||
commandRunning.first->End(false);
|
||||
commandRunning.second = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ParallelDeadlineGroup::End(bool interrupted) {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
if (commandRunning.second) {
|
||||
commandRunning.first->End(true);
|
||||
}
|
||||
}
|
||||
isRunning = false;
|
||||
}
|
||||
|
||||
bool ParallelDeadlineGroup::IsFinished() { return m_deadline->IsFinished(); }
|
||||
|
||||
bool ParallelDeadlineGroup::RunsWhenDisabled() const {
|
||||
return m_runWhenDisabled;
|
||||
}
|
||||
|
||||
void ParallelDeadlineGroup::AddCommands(
|
||||
std::vector<std::unique_ptr<Command>>&& commands) {
|
||||
if (!RequireUngrouped(commands)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (isRunning) {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"Commands cannot be added to a CommandGroup "
|
||||
"while the group is running");
|
||||
}
|
||||
|
||||
for (auto&& command : commands) {
|
||||
if (RequirementsDisjoint(this, command.get())) {
|
||||
command->SetGrouped(true);
|
||||
AddRequirements(command->GetRequirements());
|
||||
m_runWhenDisabled &= command->RunsWhenDisabled();
|
||||
m_commands[std::move(command)] = false;
|
||||
} else {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"Multiple commands in a parallel group cannot "
|
||||
"require the same subsystems");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
|
||||
m_deadline = deadline.get();
|
||||
m_deadline->SetGrouped(true);
|
||||
m_commands[std::move(deadline)] = false;
|
||||
AddRequirements(m_deadline->GetRequirements());
|
||||
m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
|
||||
}
|
||||
@@ -1,69 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/ParallelRaceGroup.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
ParallelRaceGroup::ParallelRaceGroup(
|
||||
std::vector<std::unique_ptr<Command>>&& commands) {
|
||||
AddCommands(std::move(commands));
|
||||
}
|
||||
|
||||
void ParallelRaceGroup::Initialize() {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
commandRunning->Initialize();
|
||||
}
|
||||
isRunning = true;
|
||||
}
|
||||
|
||||
void ParallelRaceGroup::Execute() {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
commandRunning->Execute();
|
||||
if (commandRunning->IsFinished()) {
|
||||
m_finished = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ParallelRaceGroup::End(bool interrupted) {
|
||||
for (auto& commandRunning : m_commands) {
|
||||
commandRunning->End(!commandRunning->IsFinished());
|
||||
}
|
||||
isRunning = false;
|
||||
}
|
||||
|
||||
bool ParallelRaceGroup::IsFinished() { return m_finished; }
|
||||
|
||||
bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; }
|
||||
|
||||
void ParallelRaceGroup::AddCommands(
|
||||
std::vector<std::unique_ptr<Command>>&& commands) {
|
||||
if (!RequireUngrouped(commands)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (isRunning) {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"Commands cannot be added to a CommandGroup "
|
||||
"while the group is running");
|
||||
}
|
||||
|
||||
for (auto&& command : commands) {
|
||||
if (RequirementsDisjoint(this, command.get())) {
|
||||
command->SetGrouped(true);
|
||||
AddRequirements(command->GetRequirements());
|
||||
m_runWhenDisabled &= command->RunsWhenDisabled();
|
||||
m_commands.emplace(std::move(command));
|
||||
} else {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"Multiple commands in a parallel group cannot "
|
||||
"require the same subsystems");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/PerpetualCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
PerpetualCommand::PerpetualCommand(std::unique_ptr<Command>&& command) {
|
||||
if (!CommandGroupBase::RequireUngrouped(command)) {
|
||||
return;
|
||||
}
|
||||
m_command = std::move(command);
|
||||
m_command->SetGrouped(true);
|
||||
AddRequirements(m_command->GetRequirements());
|
||||
}
|
||||
|
||||
void PerpetualCommand::Initialize() { m_command->Initialize(); }
|
||||
|
||||
void PerpetualCommand::Execute() { m_command->Execute(); }
|
||||
|
||||
void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); }
|
||||
@@ -1,16 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/PrintCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
PrintCommand::PrintCommand(const wpi::Twine& message)
|
||||
: CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
|
||||
}
|
||||
|
||||
bool PrintCommand::RunsWhenDisabled() const { return true; }
|
||||
@@ -1,67 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/ProfiledPIDCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
using State = frc::TrapezoidProfile::State;
|
||||
|
||||
ProfiledPIDCommand::ProfiledPIDCommand(
|
||||
frc::ProfiledPIDController controller,
|
||||
std::function<units::meter_t()> measurementSource,
|
||||
std::function<State()> goalSource,
|
||||
std::function<void(double, State)> useOutput,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_controller{controller},
|
||||
m_measurement{std::move(measurementSource)},
|
||||
m_goal{std::move(goalSource)},
|
||||
m_useOutput{std::move(useOutput)} {
|
||||
AddRequirements(requirements);
|
||||
}
|
||||
|
||||
ProfiledPIDCommand::ProfiledPIDCommand(
|
||||
frc::ProfiledPIDController controller,
|
||||
std::function<units::meter_t()> measurementSource,
|
||||
std::function<units::meter_t()> goalSource,
|
||||
std::function<void(double, State)> useOutput,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: ProfiledPIDCommand(controller, measurementSource,
|
||||
[&goalSource]() {
|
||||
return State{goalSource(), 0_mps};
|
||||
},
|
||||
useOutput, requirements) {}
|
||||
|
||||
ProfiledPIDCommand::ProfiledPIDCommand(
|
||||
frc::ProfiledPIDController controller,
|
||||
std::function<units::meter_t()> measurementSource, State goal,
|
||||
std::function<void(double, State)> useOutput,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
|
||||
useOutput, requirements) {}
|
||||
|
||||
ProfiledPIDCommand::ProfiledPIDCommand(
|
||||
frc::ProfiledPIDController controller,
|
||||
std::function<units::meter_t()> measurementSource, units::meter_t goal,
|
||||
std::function<void(double, State)> useOutput,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
|
||||
useOutput, requirements) {}
|
||||
|
||||
void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
|
||||
|
||||
void ProfiledPIDCommand::Execute() {
|
||||
m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
|
||||
m_controller.GetSetpoint());
|
||||
}
|
||||
|
||||
void ProfiledPIDCommand::End(bool interrupted) {
|
||||
m_useOutput(0, State{0_m, 0_mps});
|
||||
}
|
||||
|
||||
frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
|
||||
return m_controller;
|
||||
}
|
||||
@@ -1,36 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/ProfiledPIDSubsystem.h"
|
||||
|
||||
using namespace frc2;
|
||||
using State = frc::TrapezoidProfile::State;
|
||||
|
||||
ProfiledPIDSubsystem::ProfiledPIDSubsystem(
|
||||
frc::ProfiledPIDController controller)
|
||||
: m_controller{controller} {}
|
||||
|
||||
void ProfiledPIDSubsystem::Periodic() {
|
||||
if (m_enabled) {
|
||||
UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
|
||||
m_controller.GetSetpoint());
|
||||
}
|
||||
}
|
||||
|
||||
void ProfiledPIDSubsystem::Enable() {
|
||||
m_controller.Reset();
|
||||
m_enabled = true;
|
||||
}
|
||||
|
||||
void ProfiledPIDSubsystem::Disable() {
|
||||
UseOutput(0, State{0_m, 0_mps});
|
||||
m_enabled = false;
|
||||
}
|
||||
|
||||
frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
|
||||
return m_controller;
|
||||
}
|
||||
@@ -1,37 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/ProxyScheduleCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
|
||||
SetInsert(m_toSchedule, toSchedule);
|
||||
}
|
||||
|
||||
void ProxyScheduleCommand::Initialize() {
|
||||
for (auto* command : m_toSchedule) {
|
||||
command->Schedule();
|
||||
}
|
||||
}
|
||||
|
||||
void ProxyScheduleCommand::End(bool interrupted) {
|
||||
if (interrupted) {
|
||||
for (auto* command : m_toSchedule) {
|
||||
command->Cancel();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ProxyScheduleCommand::Execute() {
|
||||
m_finished = true;
|
||||
for (auto* command : m_toSchedule) {
|
||||
m_finished &= !command->IsScheduled();
|
||||
}
|
||||
}
|
||||
|
||||
bool ProxyScheduleCommand::IsFinished() { return m_finished; }
|
||||
@@ -1,117 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/RamseteCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
using namespace units;
|
||||
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
}
|
||||
|
||||
RamseteCommand::RamseteCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::RamseteController controller, volt_t ks,
|
||||
units::unit_t<voltsecondspermeter> kv,
|
||||
units::unit_t<voltsecondssquaredpermeter> ka,
|
||||
frc::DifferentialDriveKinematics kinematics,
|
||||
std::function<units::meters_per_second_t()> leftSpeed,
|
||||
std::function<units::meters_per_second_t()> rightSpeed,
|
||||
frc2::PIDController leftController, frc2::PIDController rightController,
|
||||
std::function<void(volt_t, volt_t)> output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_controller(controller),
|
||||
m_ks(ks),
|
||||
m_kv(kv),
|
||||
m_ka(ka),
|
||||
m_kinematics(kinematics),
|
||||
m_leftSpeed(leftSpeed),
|
||||
m_rightSpeed(rightSpeed),
|
||||
m_leftController(std::make_unique<frc2::PIDController>(leftController)),
|
||||
m_rightController(std::make_unique<frc2::PIDController>(rightController)),
|
||||
m_outputVolts(output),
|
||||
m_usePID(true) {
|
||||
AddRequirements(requirements);
|
||||
}
|
||||
|
||||
RamseteCommand::RamseteCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::RamseteController controller,
|
||||
frc::DifferentialDriveKinematics kinematics,
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_controller(controller),
|
||||
m_ks(0),
|
||||
m_kv(0),
|
||||
m_ka(0),
|
||||
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);
|
||||
m_prevSpeeds = m_kinematics.ToWheelSpeeds(
|
||||
frc::ChassisSpeeds{initialState.velocity, 0_mps,
|
||||
initialState.velocity * initialState.curvature});
|
||||
m_timer.Reset();
|
||||
m_timer.Start();
|
||||
if (m_usePID) {
|
||||
m_leftController->Reset();
|
||||
m_rightController->Reset();
|
||||
}
|
||||
}
|
||||
|
||||
void RamseteCommand::Execute() {
|
||||
auto curTime = m_timer.Get();
|
||||
auto dt = curTime - m_prevTime;
|
||||
|
||||
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
|
||||
m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
|
||||
|
||||
if (m_usePID) {
|
||||
auto leftFeedforward =
|
||||
m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
|
||||
m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
|
||||
|
||||
auto rightFeedforward =
|
||||
m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
|
||||
m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
|
||||
|
||||
auto leftOutput =
|
||||
volt_t(m_leftController->Calculate(
|
||||
m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
|
||||
leftFeedforward;
|
||||
|
||||
auto rightOutput = volt_t(m_rightController->Calculate(
|
||||
m_rightSpeed().to<double>(),
|
||||
targetWheelSpeeds.right.to<double>())) +
|
||||
rightFeedforward;
|
||||
|
||||
m_outputVolts(leftOutput, rightOutput);
|
||||
} else {
|
||||
m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
|
||||
}
|
||||
|
||||
m_prevTime = curTime;
|
||||
m_prevSpeeds = targetWheelSpeeds;
|
||||
}
|
||||
|
||||
void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
|
||||
|
||||
bool RamseteCommand::IsFinished() {
|
||||
return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/RunCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
RunCommand::RunCommand(std::function<void()> toRun,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_toRun{std::move(toRun)} {
|
||||
AddRequirements(requirements);
|
||||
}
|
||||
|
||||
void RunCommand::Execute() { m_toRun(); }
|
||||
@@ -1,24 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/ScheduleCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
ScheduleCommand::ScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
|
||||
SetInsert(m_toSchedule, toSchedule);
|
||||
}
|
||||
|
||||
void ScheduleCommand::Initialize() {
|
||||
for (auto command : m_toSchedule) {
|
||||
command->Schedule();
|
||||
}
|
||||
}
|
||||
|
||||
bool ScheduleCommand::IsFinished() { return true; }
|
||||
|
||||
bool ScheduleCommand::RunsWhenDisabled() const { return true; }
|
||||
@@ -1,75 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/SequentialCommandGroup.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
SequentialCommandGroup::SequentialCommandGroup(
|
||||
std::vector<std::unique_ptr<Command>>&& commands) {
|
||||
AddCommands(std::move(commands));
|
||||
}
|
||||
|
||||
void SequentialCommandGroup::Initialize() {
|
||||
m_currentCommandIndex = 0;
|
||||
|
||||
if (!m_commands.empty()) {
|
||||
m_commands[0]->Initialize();
|
||||
}
|
||||
}
|
||||
|
||||
void SequentialCommandGroup::Execute() {
|
||||
if (m_commands.empty()) return;
|
||||
|
||||
auto& currentCommand = m_commands[m_currentCommandIndex];
|
||||
|
||||
currentCommand->Execute();
|
||||
if (currentCommand->IsFinished()) {
|
||||
currentCommand->End(false);
|
||||
m_currentCommandIndex++;
|
||||
if (m_currentCommandIndex < m_commands.size()) {
|
||||
m_commands[m_currentCommandIndex]->Initialize();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SequentialCommandGroup::End(bool interrupted) {
|
||||
if (interrupted && !m_commands.empty() &&
|
||||
m_currentCommandIndex != invalid_index &&
|
||||
m_currentCommandIndex < m_commands.size()) {
|
||||
m_commands[m_currentCommandIndex]->End(interrupted);
|
||||
}
|
||||
m_currentCommandIndex = invalid_index;
|
||||
}
|
||||
|
||||
bool SequentialCommandGroup::IsFinished() {
|
||||
return m_currentCommandIndex == m_commands.size();
|
||||
}
|
||||
|
||||
bool SequentialCommandGroup::RunsWhenDisabled() const {
|
||||
return m_runWhenDisabled;
|
||||
}
|
||||
|
||||
void SequentialCommandGroup::AddCommands(
|
||||
std::vector<std::unique_ptr<Command>>&& commands) {
|
||||
if (!RequireUngrouped(commands)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (m_currentCommandIndex != invalid_index) {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"Commands cannot be added to a CommandGroup "
|
||||
"while the group is running");
|
||||
}
|
||||
|
||||
for (auto&& command : commands) {
|
||||
command->SetGrouped(true);
|
||||
AddRequirements(command->GetRequirements());
|
||||
m_runWhenDisabled &= command->RunsWhenDisabled();
|
||||
m_commands.emplace_back(std::move(command));
|
||||
}
|
||||
}
|
||||
@@ -1,27 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/StartEndCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
StartEndCommand::StartEndCommand(std::function<void()> onInit,
|
||||
std::function<void()> onEnd,
|
||||
std::initializer_list<Subsystem*> 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;
|
||||
m_onEnd = other.m_onEnd;
|
||||
}
|
||||
|
||||
void StartEndCommand::Initialize() { m_onInit(); }
|
||||
|
||||
void StartEndCommand::End(bool interrupted) { m_onEnd(); }
|
||||
@@ -1,27 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/Subsystem.h"
|
||||
|
||||
using namespace frc2;
|
||||
Subsystem::~Subsystem() {
|
||||
CommandScheduler::GetInstance().UnregisterSubsystem(this);
|
||||
}
|
||||
|
||||
void Subsystem::Periodic() {}
|
||||
|
||||
Command* Subsystem::GetDefaultCommand() const {
|
||||
return CommandScheduler::GetInstance().GetDefaultCommand(this);
|
||||
}
|
||||
|
||||
Command* Subsystem::GetCurrentCommand() const {
|
||||
return CommandScheduler::GetInstance().Requiring(this);
|
||||
}
|
||||
|
||||
void Subsystem::Register() {
|
||||
return CommandScheduler::GetInstance().RegisterSubsystem(this);
|
||||
}
|
||||
@@ -1,66 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/SubsystemBase.h"
|
||||
|
||||
#include <frc/smartdashboard/SendableBuilder.h>
|
||||
#include <frc/smartdashboard/SendableRegistry.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
SubsystemBase::SubsystemBase() {
|
||||
frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
|
||||
CommandScheduler::GetInstance().RegisterSubsystem({this});
|
||||
}
|
||||
|
||||
void SubsystemBase::InitSendable(frc::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Subsystem");
|
||||
builder.AddBooleanProperty(".hasDefault",
|
||||
[this] { return GetDefaultCommand() != nullptr; },
|
||||
nullptr);
|
||||
builder.AddStringProperty(".default",
|
||||
[this]() -> std::string {
|
||||
auto command = GetDefaultCommand();
|
||||
if (command == nullptr) return "none";
|
||||
return command->GetName();
|
||||
},
|
||||
nullptr);
|
||||
builder.AddBooleanProperty(".hasCommand",
|
||||
[this] { return GetCurrentCommand() != nullptr; },
|
||||
nullptr);
|
||||
builder.AddStringProperty(".command",
|
||||
[this]() -> std::string {
|
||||
auto command = GetCurrentCommand();
|
||||
if (command == nullptr) return "none";
|
||||
return command->GetName();
|
||||
},
|
||||
nullptr);
|
||||
}
|
||||
|
||||
std::string SubsystemBase::GetName() const {
|
||||
return frc::SendableRegistry::GetInstance().GetName(this);
|
||||
}
|
||||
|
||||
void SubsystemBase::SetName(const wpi::Twine& name) {
|
||||
frc::SendableRegistry::GetInstance().SetName(this, name);
|
||||
}
|
||||
|
||||
std::string SubsystemBase::GetSubsystem() const {
|
||||
return frc::SendableRegistry::GetInstance().GetSubsystem(this);
|
||||
}
|
||||
|
||||
void SubsystemBase::SetSubsystem(const wpi::Twine& name) {
|
||||
frc::SendableRegistry::GetInstance().SetSubsystem(this, name);
|
||||
}
|
||||
|
||||
void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
|
||||
auto& registry = frc::SendableRegistry::GetInstance();
|
||||
registry.AddLW(child, GetSubsystem(), name);
|
||||
registry.AddChild(this, child);
|
||||
}
|
||||
@@ -1,35 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/TrapezoidProfileCommand.h"
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
TrapezoidProfileCommand::TrapezoidProfileCommand(
|
||||
frc::TrapezoidProfile profile,
|
||||
std::function<void(frc::TrapezoidProfile::State)> output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_profile(profile), m_output(output) {
|
||||
AddRequirements(requirements);
|
||||
}
|
||||
|
||||
void TrapezoidProfileCommand::Initialize() {
|
||||
m_timer.Reset();
|
||||
m_timer.Start();
|
||||
}
|
||||
|
||||
void TrapezoidProfileCommand::Execute() {
|
||||
m_output(m_profile.Calculate(m_timer.Get()));
|
||||
}
|
||||
|
||||
void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
|
||||
|
||||
bool TrapezoidProfileCommand::IsFinished() {
|
||||
return m_timer.HasPeriodPassed(m_profile.TotalTime());
|
||||
}
|
||||
@@ -1,26 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/WaitCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
WaitCommand::WaitCommand(units::second_t duration) : m_duration{duration} {
|
||||
auto durationStr = std::to_string(duration.to<double>());
|
||||
SetName(wpi::Twine(GetName()) + ": " + wpi::Twine(durationStr) + " seconds");
|
||||
}
|
||||
|
||||
void WaitCommand::Initialize() {
|
||||
m_timer.Reset();
|
||||
m_timer.Start();
|
||||
}
|
||||
|
||||
void WaitCommand::End(bool interrupted) { m_timer.Stop(); }
|
||||
|
||||
bool WaitCommand::IsFinished() { return m_timer.HasPeriodPassed(m_duration); }
|
||||
|
||||
bool WaitCommand::RunsWhenDisabled() const { return true; }
|
||||
@@ -1,20 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/WaitUntilCommand.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
|
||||
: m_condition{std::move(condition)} {}
|
||||
|
||||
WaitUntilCommand::WaitUntilCommand(double time)
|
||||
: m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {}
|
||||
|
||||
bool WaitUntilCommand::IsFinished() { return m_condition(); }
|
||||
|
||||
bool WaitUntilCommand::RunsWhenDisabled() const { return true; }
|
||||
@@ -1,57 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/button/Button.h"
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
Button::Button(std::function<bool()> isPressed) : Trigger(isPressed) {}
|
||||
|
||||
Button Button::WhenPressed(Command* command, bool interruptible) {
|
||||
WhenActive(command, interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Button Button::WhenPressed(std::function<void()> toRun) {
|
||||
WhenActive(std::move(toRun));
|
||||
return *this;
|
||||
}
|
||||
|
||||
Button Button::WhileHeld(Command* command, bool interruptible) {
|
||||
WhileActiveContinous(command, interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Button Button::WhileHeld(std::function<void()> toRun) {
|
||||
WhileActiveContinous(std::move(toRun));
|
||||
return *this;
|
||||
}
|
||||
|
||||
Button Button::WhenHeld(Command* command, bool interruptible) {
|
||||
WhileActiveOnce(command, interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Button Button::WhenReleased(Command* command, bool interruptible) {
|
||||
WhenInactive(command, interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Button Button::WhenReleased(std::function<void()> toRun) {
|
||||
WhenInactive(std::move(toRun));
|
||||
return *this;
|
||||
}
|
||||
|
||||
Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
|
||||
ToggleWhenActive(command, interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Button Button::CancelWhenPressed(Command* command) {
|
||||
CancelWhenActive(command);
|
||||
return *this;
|
||||
}
|
||||
@@ -1,119 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 "frc2/command/button/Trigger.h"
|
||||
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
|
||||
|
||||
Trigger Trigger::WhenActive(Command* command, bool interruptible) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this, command, interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (!pressedLast && pressed) {
|
||||
command->Schedule(interruptible);
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Trigger Trigger::WhenActive(std::function<void()> toRun) {
|
||||
return WhenActive(InstantCommand(std::move(toRun), {}));
|
||||
}
|
||||
|
||||
Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this, command, interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (pressed) {
|
||||
command->Schedule(interruptible);
|
||||
} else if (pressedLast && !pressed) {
|
||||
command->Cancel();
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
|
||||
Trigger Trigger::WhileActiveContinous(std::function<void()> toRun) {
|
||||
return WhileActiveContinous(InstantCommand(std::move(toRun), {}));
|
||||
}
|
||||
|
||||
Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this, command, interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (!pressedLast && pressed) {
|
||||
command->Schedule(interruptible);
|
||||
} else if (pressedLast && !pressed) {
|
||||
command->Cancel();
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
|
||||
Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this, command, interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (pressedLast && !pressed) {
|
||||
command->Schedule(interruptible);
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
|
||||
Trigger Trigger::WhenInactive(std::function<void()> toRun) {
|
||||
return WhenInactive(InstantCommand(std::move(toRun), {}));
|
||||
}
|
||||
|
||||
Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this, command, interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (!pressedLast && pressed) {
|
||||
if (command->IsScheduled()) {
|
||||
command->Cancel();
|
||||
} else {
|
||||
command->Schedule(interruptible);
|
||||
}
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
|
||||
Trigger Trigger::CancelWhenActive(Command* command) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this, command]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (!pressedLast && pressed) {
|
||||
command->Cancel();
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
@@ -12,7 +12,7 @@
|
||||
#include <networktables/NetworkTableInstance.h>
|
||||
#include <wpi/mutex.h>
|
||||
|
||||
#include "frc/commands/Scheduler.h"
|
||||
#include "frc/smartdashboard/Sendable.h"
|
||||
#include "frc/smartdashboard/SendableBuilderImpl.h"
|
||||
#include "frc/smartdashboard/SendableRegistry.h"
|
||||
|
||||
@@ -99,19 +99,15 @@ bool LiveWindow::IsEnabled() const {
|
||||
void LiveWindow::SetEnabled(bool enabled) {
|
||||
std::scoped_lock lock(m_impl->mutex);
|
||||
if (m_impl->liveWindowEnabled == enabled) return;
|
||||
Scheduler* scheduler = Scheduler::GetInstance();
|
||||
m_impl->startLiveWindow = enabled;
|
||||
m_impl->liveWindowEnabled = enabled;
|
||||
// Force table generation now to make sure everything is defined
|
||||
UpdateValuesUnsafe();
|
||||
if (enabled) {
|
||||
scheduler->SetEnabled(false);
|
||||
scheduler->RemoveAll();
|
||||
} else {
|
||||
m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
|
||||
cbdata.builder.StopLiveWindowMode();
|
||||
});
|
||||
scheduler->SetEnabled(true);
|
||||
}
|
||||
m_impl->enabledEntry.SetBoolean(enabled);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user