Subsumption geht auch, relativ kompakt, ohne multi threading oder state transition Technik.

Hier mal mein Beispiel Code, mit dem sich ASURO wacker schlägt.
Code:
#include "asuro.h"

typedef int (*FunctionPointer) (int);
FunctionPointer actionList[];

int slow=60;
int fast=80;
unsigned char currentTask;

int wait(int msTime) {
	long t1=Gettime()+msTime;
	unsigned char newTask, savedTask=currentTask;
	int sensor, action=0;
		
	do {
		for(newTask=0; newTask<savedTask; ) {
			sensor=(*actionList[2*newTask])(newTask);
			if(sensor) {
				currentTask=newTask;
				action|=(*actionList[2*newTask+1])(sensor);
				newTask=0;
			} else newTask++;
		}
	} while(t1 > Gettime());
	currentTask=savedTask; 
	return action;
}

void drive(int leftSpeed_, int rightSpeed_) {
	SetMotorPower(leftSpeed_, rightSpeed_);
}

int blink_SecTimer(int idx) {
	static int t1=0;
	int t0=t1;
	t1=(Gettime()/1000)&0x1;
	return t0^t1; // bei Sekundenwechsel ==> 1 sonst 0
}

int blink_Action(int sensor) {
	static int led=GREEN;
	led=(led==GREEN) ? RED : GREEN;
	StatusLED(led);
	return 0x0;
}

int avoid_Obstacle(int idx) {
	return PollSwitch();
}

int avoid_Action(int sensor) {
	drive(-slow, -slow); // uups erst mal 2 sec zurueck
	wait(2000);
	if(sensor<16) drive(-slow, 0); else drive(0, -slow); // dreh dich weg vom Hindernis
	wait(1000);
	return 0x1;
}

int cruise_Forever(int idx) {
	return 1;
}

int cruise_Action(int sensor) {
	drive(fast, fast); // fahr rum
	return 0x8;
}

FunctionPointer actionList[] ={
	/* sense	  ,	action */
	//motor_Awake,	motor_Control,
	blink_SecTimer,	blink_Action,
	avoid_Obstacle,	avoid_Action,
	cruise_Forever,	cruise_Action,
};
	
int main(void) {
	Init();	
	EncoderInit();
	EncoderStart();
	
	drive(0,0);
	mSleep(1000);
	currentTask=sizeof(actionList)/sizeof(FunctionPointer)/2;
	return wait(0);		
}
ASURO wird dadurch wie folgt gesteuert:
  • Task blink prio0 (höchste): Die Statuslampe blinkt im Sekundentakt.
    Task avoid prio1: ASURO weicht bei einer Kollision aus.
    Task cruise prio2 (niedrigste): ASURO fährt grade aus.

Jeder Task besteht aus zwei C-Funktionen.
a) Einem Sensor-Teil der entscheidet ob der Action-Teil ausgeführt werden soll.
b) Einem Action-Teil der die entsprechende Aktion durchführt.

Läuft ein Task hoher Priorität blockiert er solange alle Tasks niedrigerer Priorität bis er fertig ist (= seine Action-Funktion verlassen wird).

Ein Task ist kooperativ und gibt höher priorisierten Task die Gelegenheit zur Ausführung indem er (möglichst) oft die Funktion wait(napTime) aufruft.

Die Koordination des Ganzen liegt in der Hand der Funktion wait(napTime). Diese prüft ob ein Task laufen will (dazu führt sie seine Sensor-Funktion aus) und startet dann gegebenenfalls seine Action-Funktion. Tasks werden immer streng nach Priorität ausgeführt (erinnert mich ein bischen an das Spiel: die Türme von Hanoi).

Beliebig viele Tasks (mit ihrem Sensor- und Action-Teil) werden in das Array actionList[] mit abnehmender Priorität eingestellt.