OK, mein Code sieht momentan so aus:
Code:
#include "asuro.h"
#define L_DIR ((PORTD & RWD) ? -1 : 1)   
#define R_DIR ((PORTB & RWD) ? -1 : 1)

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

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

int leftSpeed=0;
int rightSpeed=0;
long motorTime=0;
int delta=10, Kp=20; // entspricht Kp=0.2

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_) 
{
   leftSpeed  = leftSpeed_;
   rightSpeed = rightSpeed_;
}

int regeln(int pwm, int e) 
{
   int y = (Kp * e)/100;
   y= (y>10) ? 10 : ((y<-10) ? -10 : y);
   pwm += y;
   return (pwm>127) ? 127 : ((pwm<-127) ? -127 : pwm);
}

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 motor_Awake(int idx) 
{
   return Gettime()>motorTime;
}

int motor_Control(int sensor) 
{
   static int lpwm=0;
   static int rpwm=0;
   int l_ticks, r_ticks;
   int Ta = Gettime() - motorTime;

   l_ticks = encoder[LEFT];
   r_ticks = encoder[RIGHT];
   EncoderSet(0,0); //Encoder Reset
   motorTime = Gettime() + delta;
   
   lpwm = regeln(lpwm, leftSpeed  -8000L*L_DIR*l_ticks/Ta);
   rpwm = regeln(rpwm, rightSpeed -8000L*R_DIR*r_ticks/Ta);
      
   SetMotorPower(lpwm, rpwm);
   return 0x1;
} 

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(200); // 0,2 sec Rückwaertskurve (abwechselnd links und rechts)
   return 0x2;
}

int cruise_Forever(int idx)
{
   return 1;
}

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

FunctionPointer actionList[] =
{
   /* sense      ,   action */
   blink_SecTimer,   blink_Action ,
   motor_Awake   ,   motor_Control,
   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);     
}
Die 8000 habe ich genommen, um die Priorität höher zu setzen.
Das erscheint mir momentan noch das Problem. Wie steuert man nachvollziehbar die Priorität. Im Programmgerüst des c't-Bot konnte man diese konkret angeben. Vielleicht kannst Du meinen Code mal testen. Wo würdest Du ansetzen, um ihn weiter zu optimieren?