Das Init() fehlt noch und ich würde dies ergänzen:

StatusLED(RED);
while(PollSwitch()!=0);
}

Wenn PollSwitch() richtige Werte liefert sollte das funktionieren.