File content as of revision 0:823a9a4db739:

#include "mbed.h"
#include "Constants.h"
#include "OS.h"
extern Serial debug;

pTask TaskList[NumTask];

int SCHAddTask(void(*fP)(void), int P, int D)
{
    int i = 0;
    while(i <= NumTask && TaskList[i].fP != 0)
    { 
        i++;
    }
    if (i > NumTask)
    {
        return ArrayFull;
    }
    TaskList[i].fP = fP;
    TaskList[i].Delay = D;
    TaskList[i].Period = P;
    TaskList[i].RunMe = 0;
    
    return i;
}

int SCHRemoveTask(int i)
{
    if (TaskList[i].fP == 0)
    {
        return NothingFound;
    }
    TaskList[i].fP = 0;
    TaskList[i].Delay = 0;
    TaskList[i].Period = 0;
    TaskList[i].RunMe = 0;
    return i;
}

void SCHDispatch()
{
debug.printf("Entered Dispatch");
int i = 0;
    for (i = 0; i <NumTask; i++)
    {
        if (TaskList[i].RunMe > 0)
        {
            TaskList[i].fP();
            TaskList[i].RunMe--;
        }
    }
} 

void SCHUpdate()
{
debug.printf("Interrupted \n \r");
int i = 0;
for (i = 0; i < NumTask; i++) //run through the whole tasklist array.
    {
        TaskList[i].Delay--;
        if (TaskList[i].Delay <=0)
            {
                TaskList[i].RunMe++; //Add 1 to runme per timeout.
                TaskList[i].Delay = TaskList[i].Period; //reset the timeout to the period.
            }
    }
}