Mit Ultraschallecholot funktioniert das auch recht gut:
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;
   if(led==GREEN)
      led=RED;
   else
	  led=GREEN;
   StatusLED(led);
   return 0x0;
}

int avoid_Obstacle(int idx) 
{
   int abstand = Chirp();
   if (abstand<10)
   { 
     BackLED(ON,ON);
	 return abstand;
   }
   else
   {
     BackLED(OFF,OFF); 
     return 0;
   }
}

int avoid_Action(int sensor) 
{
   static int flag;
   drive(-slow, -slow); // 0,5 sec zurueck
   wait(500);
   if(flag) 
   {
     drive(-slow, 0);
	 flag=0;
   } 
   else 
   {
     drive(0, -slow); 
	 flag=1;
   }
   wait(1000); // 1 sec Rückwaertskurve (abwechselnd links und rechts)
   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);      
}