File content as of revision 0:301a83d3eeb4:

#include "mbed.h"

Serial pc(USBTX,USBRX);

int taskCount = 0;
const int maxTasks = 16;
void *taskList[maxTasks];

typedef void function_t(void);

//runs the next task. Avoid calling from a ticker since that would block all interrupts while running.
void nextTask(void)
{
    static int taskID = 0;
    if (taskCount) {
        if (taskID >= taskCount) // a task could have been removed.
            taskID = 0;

        ((function_t*)taskList[taskID])();
        taskID++;
        if (taskID == taskCount)
            taskID = 0;
    }
}

// add a task to the list
bool addTask(void (*function)(void))
{
    if ((taskCount < maxTasks) && function) {
        taskList[taskCount] = (void *)function;
        taskCount++;
        return true;
    } else
        return false;
}

// finds a task in the list or -1 if not in the list. Mainly used for removing tasks.
int findTask(void (*function)(void))
{
    if (!function)
        return -1;
    int number = 0;
    while (number < taskCount) {
        if (taskList[number] == (void *)function)
            return number;
        number++;
    }
    return -1;
}

// remove a task. returns false if the task wasn't in the list.
bool removeTask(void (*function)(void))
{
    int taskToRemove = findTask(function);
    if (taskToRemove == -1)
        return false;

    for (int i = taskToRemove; i<(taskCount-1); i++) {
        taskList[i] = taskList[i+1];
    }
    taskCount--;
    return true;
}

// simple example task.
void task1(void)
{
    const int NumberOfStates = 5;
    static int state = 0;
    switch (state) {
        case 0:
            pc.putc('A');
            break;
        case 1:
            pc.putc('B');
            break;
        case 2:
            pc.putc('C');
            break;
        case 3:
            pc.putc('D');
            break;
        case 4:
            pc.putc('E');
            break;
        default:
            state = 0;
            break;
    }
    state++;
    if (state == NumberOfStates)
        state = 0;
}

// simple example task.
void task2(void)
{
    const int NumberOfStates = 2;
    static int state = 0;
    switch (state) {
        case 0:
            pc.putc('1');
            break;
        case 1:
            pc.putc('2');
            break;
        default:
            state = 0;
            break;
    }
    state++;
    if (state == NumberOfStates)
        state = 0;
}

// simple example task using enum for current state.
void task3(void)
{
    static enum {firstState,secondState} state = firstState;
    switch (state) {
        case firstState:
            pc.putc('X');
            state = secondState;
            break;
        case secondState:
            pc.putc('Y');
            state = firstState;
            break;
        default:
            state = firstState;
            break;
    }
}

main ()
{

    addTask(&task1);
    addTask(&task2);
    pc.printf("\r\nTasks 1 & 2\r\n");
    for (int i = 0; i < 20; i++)
        nextTask();


    addTask(&task1);
    addTask(&task3);
    pc.printf("\r\nTasks 1,2,1,3\r\n");
    for (int i = 0; i < 20; i++)
        nextTask();

    removeTask(&task1); // removes the first instance of task 1 in the list.
    pc.printf("\r\nTasks 2,1,3\r\n");
    for (int i = 0; i < 20; i++)
        nextTask();

    pc.printf("\r\nDone\r\n");

}