Bau zu Diagnosezwecken noch LEDs ein, damit Du weißt, ob alle Zweige erreicht werden, z.B.:

Code:
if (PollSwitch()>0)
{
    MotorDir(BREAK,BREAK);
    MotorSpeed(0,0);
    BackLED(ON,ON); // nur zu Diagnosezwecken
}