Natürlich vll hat ja jemand noch eine Verbesserung Vorschlag dafür.
Ich arbeite grade schon an der Odometrie.
Code:
int main(void)
{
    Init();
    MotorSpeed(0,0);
    MotorDir(FWD,FWD);
    unsigned char sw;
    MotorFWD();

while(1)
{

    sw = PollSwitch();

    if (sw==PollSwitch() && (sw & 0x3F))  { Back(); }
    else { MotorFWD(); }

}
return 0;
}


void MotorFWD(void)
{
     MotorDir(FWD,FWD);
     MotorSpeed(120,120);
     FrontLED(ON);
     return 0;
  }

void Turn (void)
{
     MotorDir(FWD,RWD);
     MotorSpeed(120,120);
     pSleep(350);
     StatusLED(OFF);
     FrontLED(OFF);
     BackLED(OFF,OFF);
     return 0;
}

void Back (void)
{
 
     MotorDir(RWD,RWD);
     MotorSpeed(120,120);
     BackLED(ON,ON);
     StatusLED(GREEN);
     FrontLED(ON);
     pSleep(500);

     Turn();
   return 0;
}

void pSleep(int p)
{
     int i;
     for(i=0;i<=p;i++) { Sleep(72); }
     return 0;
}