if (sw & 0x01)
MotorDir(BREAK,BREAK);
if (sw & 0x02)
MotorDir(BREAK,BREAK);
if (sw & 0x04)
MotorDir(BREAK,BREAK);
if (sw & 0x0
MotorDir(BREAK,BREAK);
if (sw & 0x10)
MotorDir(BREAK,BREAK);
if (sw & 0x20)
MotorDir(BREAK,BREAK);
Du fragst hier alle Taster einzeln ab, das brauchst du garnicht, du kannst auch alle gleichzeitig auswerten.
--> if (PollSwitch()>0)
MotorDir(BREAK,BREAK);
Außerdem müssten die letzten beiden 16 und 32 statt 10 und 20 heißen.