public void Cancel(params ICommand[] commands)
{
if (m_inRunLoop)
{
m_toCancel.AddRange(commands);
return;
}
foreach (var command in commands)
{
if (!m_scheduledCommands.ContainsKey(command))
{
continue;
}
command.End(true);
OnCommandInterrupt?.Invoke(command);
m_scheduledCommands.Remove(command);
foreach (var requirement in command.Requirements)
{
m_requirements.Remove(requirement);
}
}
}
public void Run()
{
if (m_disabled)
{
return;
}
foreach (var subystem in m_subsystems.Keys)
{
subystem.Periodic();
}
foreach (var button in m_buttons)
{
button();
}
m_inRunLoop = true;
m_toRemoveFromRunning.Clear();
foreach (var command in m_scheduledCommands.Keys)
{
if (!command.RunsWhenDisabled && RobotState.IsDisabled)
{
command.End(true);
OnCommandInterrupt?.Invoke(command);
foreach (var requirement in command.Requirements)
{
m_requirements.Remove(requirement);
}
m_toRemoveFromRunning.Add(command);
continue;
}
command.Execute();
OnCommandExecute?.Invoke(command);
if (command.IsFinished)
{
command.End(false);
OnCommandFinish?.Invoke(command);
foreach (var requirement in command.Requirements)
{
m_requirements.Remove(requirement);
}
m_toRemoveFromRunning.Add(command);
}
}
foreach (var item in m_toRemoveFromRunning)
{
m_scheduledCommands.Remove(item);
}
m_inRunLoop = false;
foreach (var commandInterruptible in m_toSchedule)
{
Schedule(commandInterruptible.Value, commandInterruptible.Key);
}
foreach (var command in m_toCancel)
{
Cancel(command);
}
m_toSchedule.Clear();
m_toCancel.Clear();
foreach (var subsystemCommand in m_subsystems)
{
if (!m_requirements.ContainsKey(subsystemCommand.Key) && subsystemCommand.Value != null)
{
Schedule(subsystemCommand.Value);
}
}
}
