wie würdet ihr folgende beiden programme miteinander verschmelzen?

PROGRAMM 1
#include "asuro.h"

#define SPEED 0x8F

int speedLeft,speedRight;
unsigned int lineData[2];
int ADOffset;

void LineLeft (void)
{
speedLeft += 1;
if (speedLeft > 0xFE) speedLeft = 0xFF;
}

void LineRight (void)
{
speedRight += 1;
if (speedRight > 0xFE) speedRight = 0xFF;
}

void LineDemo(void)
{
int i;
unsigned char j;

Init();

FrontLED(ON);
for (j = 0; j < 0xFF; j++) LineData(lineData);
LineData(lineData);
ADOffset = lineData[0] - lineData[1];
speedLeft = speedRight = SPEED;
for(;;) {
LineData(lineData);
i = (lineData[0] - lineData[1]) - ADOffset;
if ( i > 4) {
StatusLED(GREEN);
LineLeft();
}
else if ( i < -4)
StatusLED(RED);
LineRight();
}
else {
StatusLED(OFF);
speedLeft = speedRight = SPEED;
}
MotorSpeed(speedLeft,speedRight);
}
}

Dieses Programm ist ein Kollisionsprogramm, stößt Asuro auf einen Gegenstand, fährt er entweder rückwärtskurve links bzw. rückwärtskurve rechts und fährt dann wieder geradeaus...


PROGRAMM 2:
#include "asuro.h"

#define SPEED 0x8F

int speedLeft,speedRight;
unsigned int lineData[2];
int ADOffset;

void LineLeft (void)
{
speedLeft += 1;
if (speedLeft > 0xFE) speedLeft = 0xFF;
}

void LineRight (void)
{
speedRight += 1;
if (speedRight > 0xFE) speedRight = 0xFF;
}

void LineDemo(void)
{
int i;
unsigned char j;

Init();

FrontLED(ON);
for (j = 0; j < 0xFF; j++) LineData(lineData);
LineData(lineData);
ADOffset = lineData[0] - lineData[1];
speedLeft = speedRight = SPEED;
for(;;) {
LineData(lineData);
i = (lineData[0] - lineData[1]) - ADOffset;
if ( i > 4) {
StatusLED(GREEN);
LineLeft();
}
else if ( i < -4)
StatusLED(RED);
LineRight();
}
else {
StatusLED(OFF);
speedLeft = speedRight = SPEED;
}
MotorSpeed(speedLeft,speedRight);
}
}

Dies ist das typische Linienverfolgungsprogramm...

Ich möchte jetzt beide miteinander verschmelzen... wenn asuro seine linie verfolgt und auf einen gegenstand stößt, soll er, je nachdem welcher schalter betätigt wird, entweder rückwärtskurve links oder rückwärtskurve rechts ca. 5cm fahren und dann geradeaus weiter und sobald er wieder die linie findet diese weiterverfolgen!
bitte helft mir hab da arge probleme mein selbstgeproggtes funzt einfach net!

mfg the-boon-