/// <summary>
/// Add a new Periodic Task to the Scheduler
/// </summary>
/// <param name="thePeriodicTask">The Periodic Task to Add</param>
public void Add(PeriodicTask thePeriodicTask)
{
//taskQueue.Add(thePeriodicTask.Period, thePeriodicTask);
// Checks if another task has been already scheduled at the same time
// long nextDueTime = thePeriodicTask.Period;
long nextDueTime = 0;
bool scheduled = false;
while (!scheduled)
{
if (AlreadyScheduled(nextDueTime))
{
nextDueTime = nextDueTime + standardDelayWhenOverscheduled;
}
else
{
scheduled = true;
}
}
taskQueue.Add(new TaskTimingStructure(nextDueTime), thePeriodicTask);
}
/// <summary>
/// The scheduler algorithm
/// </summary>
protected void theScheduler()
{
// Sleeps until first due time
stopWatch.Reset();
stopWatch.Start();
// LcdConsole.WriteLine("Stopwatch started");
if (taskQueue.Count != 0)
{
while (keepScheduling)
{
// Gets current time task data
long msec = stopWatch.ElapsedMilliseconds;
long dueTime = taskQueue.Keys[0].nextDueTime;
long numberOfExecutions = taskQueue.Keys[0].numberOfExecutions;
// LcdConsole.WriteLine("Extracting data");
// Time to wait until next execution
int rest = (int)(dueTime - msec);
// Put this thread to sleep if the dueTime is further than 200ms
if (rest > 200)
{
// LcdConsole.WriteLine("Waiting asleep");
Thread.Sleep(rest - 200);
}
// When dueTime is closer than 200ms, this function actively waits the stopwatch
while (true)
{
// LcdConsole.WriteLine("Waiting awake");
if (stopWatch.ElapsedMilliseconds >= msec + rest)
{
break;
}
}
// When the stopwatch reaches the dueTime, the callBack function is invoked
// But before checks the the scheduler has not been stopped
// while waiting
if (keepScheduling)
{
PeriodicTask task = TaskQueue.Values[0];
if (withLog)
{
// Logs the execution time
executionLog = executionLog +
stopWatch.ElapsedMilliseconds.ToString() + " " +
task.Name + "\r\n";
}
// Executes the action
// LcdConsole.WriteLine("Launching action " + task.Name);
// LcdConsole.WriteLine("Sched error: " + Math.Abs(dueTime - stopWatch.ElapsedMilliseconds).ToString("F2"));
task.Action(robot);
// Determines next due time
// LcdConsole.WriteLine("Computes next due time");
long nextDueTime = (numberOfExecutions + 1) * task.Period;
bool slotFound = false;
// Updates the due time
PeriodicTask updatedTask = task;
taskQueue.RemoveAt(0);
while (!slotFound)
{
// Check for task already scheduled at the same time
if (AlreadyScheduled(nextDueTime))
{
// If a task is already scheduled at the same time
// try next millisecond
nextDueTime = nextDueTime + standardDelayWhenOverscheduled;
}
else
{
// First task to be scheduled at this nextDueTime slot
slotFound = true;
}
}
// schedules the task
TaskTimingStructure key = new TaskTimingStructure(nextDueTime);
key.numberOfExecutions = numberOfExecutions + 1;
taskQueue.Add(key, task);
}
}
}
stopWatch.Stop();
}
