das sollte soweit laufen,
evtl. könnte man die Pullups der Eingänge aktivieren, damit die Signale etwas stabiler werden,
also nach dem configurieren als input die Pins mit zB. Portb.0 = 1 den Pullup anmachen.
Das kommt aber drauf an wie Du das alles angeschlossen und verkabelt hast. (Schalter nach GND)