Naja, so einfach ist das nicht. Du must dazu dein Program ziemlich komplett umbauen. Dann allerdings würden z.B. auch deine vielen LineData()-Aufrufe verschwinden.

Als Anregung mal meine Anfänge aus einem älteren Thread:

Code:
#include "../inc/asuro.h" 
#include "../inc/asuro.c" 

#define schwelle 15 
#define power 175 

unsigned char pow_l, pow_r, dir_l, dir_r; 
unsigned int data[2], suchen; 
unsigned int h_l, h_r, d_l, d_r; 

int main(void) { 

Init(); 
FrontLED(ON); 
dir_l=dir_r=FWD; 
pow_l=pow_r=power; 
d_l=d_r=0; 
h_l=h_r=1023; 
MotorDir(dir_l,dir_r); 
MotorSpeed(pow_l,pow_r); 
suchen=0; 

do{ 
LineData(data); 
BackLED(OFF,OFF); 
if ((data[0]-schwelle < d_l) && (data[1]-schwelle < d_r)) { 
   dir_l=dir_r=FWD; 
   pow_l=255-data[0]; 
   pow_r=255-data[1]; 
   suchen=0; 
   StatusLED(GREEN); 
   BackLED(ON,ON); 
} else { 
   if (data[1]-schwelle > data[0]) { 
      pow_l=0; 
      pow_r=power; 
      d_l=(d_l+data[0])/2; 
      h_r=(h_r+data[1])/2; 
      suchen=1; 
      StatusLED(YELLOW); 
      BackLED(OFF,ON); 
   } 
   if (data[0]-schwelle > data[1]) { 
      pow_l=power; 
      pow_r=0; 
      h_l=(h_l+data[0])/2; 
      d_r=(d_r+data[1])/2; 
      suchen=1; 
      StatusLED(YELLOW); 
      BackLED(ON,OFF); 
   } 
   if (suchen==1) { 
      if ((h_l-schwelle < data[0]) && (h_r-schwelle < data[1])) { 
         StatusLED(RED); 
         if (pow_l==0) { dir_l=RWD; pow_l=power; } 
         if (pow_r==0) { dir_r=RWD; pow_r=power; } 
      } 
      suchen=2; 
   } 
    
} 

MotorDir(dir_l,dir_r); 
MotorSpeed(pow_l,pow_r); 

}while (1); 
return 0; 
}
Damit macht mein asuro das:
Bild hier  
Bild anklicken für youtube-Video
mit wirklicher Geschwindigkeit


Gruß

mic