internal void Remove(Command command)
{
if (command == null || !m_commandTable.ContainsKey(command))
{
return;
}
LinkedListElement e = m_commandTable[command];
m_commandTable.Remove(command);
if (e.Equals(m_lastCommand))
{
m_lastCommand = e.GetPrevious();
}
if (e.Equals(m_firstCommand))
{
m_firstCommand = e.GetNext();
}
e.Remove();
var requirements = command.GetRequirements();
foreach (var requirement in requirements)
{
requirement.SetCurrentCommand(null);
}
command.Removed();
}
public void Add(LinkedListElement l)
{
if (next == null)
{
next = l;
next.previous = this;
}
else
{
next.previous = l;
l.next = next;
l.previous = this;
next = l;
}
}
private void _Add(Command command)
{
if (command == null)
{
return;
}
if (m_adding)
{
Console.Error.WriteLine($"WARNING: Cannot start command from cancel method. Ignoring: {command}");
}
if (!m_commandTable.ContainsKey(command))
{
IEnumerable <Subsystem> requirements = command.GetRequirements();
if (requirements.Any(subsystem => subsystem.GetCurrentCommand() != null && !subsystem.GetCurrentCommand().Interruptible))
{
return;
}
m_adding = true;
requirements = command.GetRequirements();
foreach (var subsystem in requirements)
{
if (subsystem.GetCurrentCommand() != null)
{
subsystem.GetCurrentCommand().Cancel();
Remove(subsystem.GetCurrentCommand());
}
subsystem.SetCurrentCommand(command);
}
m_adding = false;
LinkedListElement element = new LinkedListElement();
element.SetData(command);
if (m_firstCommand == null)
{
m_firstCommand = m_lastCommand = element;
}
else
{
m_lastCommand.Add(element);
m_lastCommand = element;
}
m_commandTable.Add(command, element);
m_runningCommandsChanged = true;
command.StartRunning();
}
}
/// <summary>
/// Runs a single iteration of the scheduler loop.
/// </summary>
/// <remarks>This method should be called often in order to have a functioning
/// <see cref="Command"/> system. The loop has 5 stages
/// <list type="ordered">
/// <listItem><para>Poll the buttons.</para></listItem>
/// <listItem><para>Execute/Remove the commands.</para></listItem>
/// <listItem><para>Send values to the <see cref="SmartDashboard.SmartDashboard"/>.</para></listItem>
/// <listItem><para>Add Commands.</para></listItem>
/// <listItem><para>Add Defaults.</para></listItem>
/// </list></remarks>
public void Run()
{
m_runningCommandsChanged = false;
if (!m_enabled)
{
return;
}
if (m_buttons != null)
{
for (int i = m_buttons.Count - 1; i >= 0; i--)
{
m_buttons[i].Execute();
}
}
LinkedListElement e = m_firstCommand;
while (e != null)
{
Command c = e.GetData();
e = e.GetNext();
if (!c.Run())
{
Remove(c);
m_runningCommandsChanged = true;
}
}
foreach (Command t in m_additions)
{
_Add(t);
}
m_additions.Clear();
foreach (var subsystem in m_subsystems)
{
if (subsystem.GetCurrentCommand() == null)
{
_Add(subsystem.GetDefaultCommand());
}
subsystem.ConfirmCommand();
}
UpdateTable();
}
private void UpdateTable()
{
if (Table == null)
{
return;
}
double[] toCancel = Table.GetNumberArray("Cancel", new double[0]);
if (toCancel.Length > 0)
{
for (LinkedListElement e = m_firstCommand; e != null; e = e.GetNext())
{
for (int i = 0; i < toCancel.Length; i++)
{
if (e.GetData().GetHashCode() == toCancel[i])
{
e.GetData().Cancel();
}
}
}
Table.PutNumberArray("Cancel", new double[0]);
}
if (m_runningCommandsChanged)
{
int n = 0;
for (LinkedListElement e = m_firstCommand; e != null; e = e.GetNext())
{
n++;
}
string[] commands = new string[n];
double[] ids = new double[n];
n = 0;
for (LinkedListElement e = m_firstCommand; e != null; e = e.GetNext())
{
commands[n] = e.GetData().Name;
ids[n] = e.GetNext().GetHashCode();
n++;
}
Table.PutStringArray("Names", commands);
Table.PutNumberArray("Ids", ids);
}
}
public LinkedListElement Remove()
{
if (previous == null && next == null)
{
}
else if (next == null)
{
previous.next = null;
}
else if (previous == null)
{
next.previous = null;
}
else
{
next.previous = previous;
previous.next = next;
}
LinkedListElement n = next;
next = null;
previous = null;
return n;
}
private void _Add(Command command)
{
if (command == null)
{
return;
}
if (m_adding)
{
Console.Error.WriteLine($"WARNING: Cannot start command from cancel method. Ignoring: {command}");
}
if (!m_commandTable.ContainsKey(command))
{
IEnumerable<Subsystem> requirements = command.GetRequirements();
if (requirements.Any(subsystem => subsystem.GetCurrentCommand() != null && !subsystem.GetCurrentCommand().Interruptible))
{
return;
}
m_adding = true;
requirements = command.GetRequirements();
foreach (var subsystem in requirements)
{
if (subsystem.GetCurrentCommand() != null)
{
subsystem.GetCurrentCommand().Cancel();
Remove(subsystem.GetCurrentCommand());
}
subsystem.SetCurrentCommand(command);
}
m_adding = false;
LinkedListElement element = new LinkedListElement();
element.SetData(command);
if (m_firstCommand == null)
{
m_firstCommand = m_lastCommand = element;
}
else
{
m_lastCommand.Add(element);
m_lastCommand = element;
}
m_commandTable.Add(command, element);
m_runningCommandsChanged = true;
command.StartRunning();
}
}
internal void Remove(Command command)
{
if (command == null || !m_commandTable.ContainsKey(command))
{
return;
}
LinkedListElement e = m_commandTable[command];
m_commandTable.Remove(command);
if (e.Equals(m_lastCommand))
{
m_lastCommand = e.GetPrevious();
}
if (e.Equals(m_firstCommand))
{
m_firstCommand = e.GetNext();
}
e.Remove();
var requirements = command.GetRequirements();
foreach (var requirement in requirements)
{
requirement.SetCurrentCommand(null);
}
command.Removed();
}
