Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[commands] Add interruptor parameter to onCommandInterrupt callbacks (#5461)

Browse Source
This commit is contained in:
Joseph Eng
2023-09-17 20:47:37 -07:00
committed by GitHub
parent e93c233d60
commit b265a68eea
5 changed files with 245 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
View File

@@ -48,7 +48,7 @@ class CommandScheduler::Impl {
// every command.
wpi::SmallVector<Action, 4> initActions;
wpi::SmallVector<Action, 4> executeActions;
wpi::SmallVector<Action, 4> interruptActions;
wpi::SmallVector<InterruptAction, 4> interruptActions;
wpi::SmallVector<Action, 4> finishActions;
// Flag and queues for avoiding concurrent modification if commands are
@@ -56,7 +56,8 @@ class CommandScheduler::Impl {
bool inRunLoop = false;
wpi::SmallVector<Command*, 4> toSchedule;
wpi::SmallVector<Command*, 4> toCancel;
wpi::SmallVector<Command*, 4> toCancelCommands;
wpi::SmallVector<std::optional<Command*>, 4> toCancelInterruptors;
};
template <typename TMap, typename TKey>
@@ -138,7 +139,7 @@ void CommandScheduler::Schedule(Command* command) {
if (isDisjoint || allInterruptible) {
if (allInterruptible) {
for (auto&& cmdToCancel : intersection) {
Cancel(cmdToCancel);
Cancel(cmdToCancel, std::make_optional(command));
}
}
m_impl->scheduledCommands.insert(command);
@@ -196,7 +197,7 @@ void CommandScheduler::Run() {
// Run scheduled commands, remove finished commands.
for (Command* command : m_impl->scheduledCommands) {
if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
Cancel(command);
Cancel(command, std::nullopt);
continue;
}
@@ -226,12 +227,13 @@ void CommandScheduler::Run() {
Schedule(command);
}
for (auto&& command : m_impl->toCancel) {
Cancel(command);
for (size_t i = 0; i < m_impl->toCancelCommands.size(); i++) {
Cancel(m_impl->toCancelCommands[i], m_impl->toCancelInterruptors[i]);
}
m_impl->toSchedule.clear();
m_impl->toCancel.clear();
m_impl->toCancelCommands.clear();
m_impl->toCancelInterruptors.clear();
// Add default commands for un-required registered subsystems.
for (auto&& subsystem : m_impl->subsystems) {
@@ -319,13 +321,15 @@ Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
}
}
void CommandScheduler::Cancel(Command* command) {
void CommandScheduler::Cancel(Command* command,
std::optional<Command*> interruptor) {
if (!m_impl) {
return;
}
if (m_impl->inRunLoop) {
m_impl->toCancel.emplace_back(command);
m_impl->toCancelCommands.emplace_back(command);
m_impl->toCancelInterruptors.emplace_back(interruptor);
return;
}
@@ -341,11 +345,15 @@ void CommandScheduler::Cancel(Command* command) {
}
command->End(true);
for (auto&& action : m_impl->interruptActions) {
action(*command);
action(*command, interruptor);
}
m_watchdog.AddEpoch(command->GetName() + ".End(true)");
}
void CommandScheduler::Cancel(Command* command) {
Cancel(command, std::nullopt);
}
void CommandScheduler::Cancel(const CommandPtr& command) {
Cancel(command.get());
}
@@ -424,6 +432,14 @@ void CommandScheduler::OnCommandExecute(Action action) {
}
void CommandScheduler::OnCommandInterrupt(Action action) {
m_impl->interruptActions.emplace_back(
[action = std::move(action)](const Command& command,
const std::optional<Command*>& interruptor) {
action(command);
});
}
void CommandScheduler::OnCommandInterrupt(InterruptAction action) {
m_impl->interruptActions.emplace_back(std::move(action));
}