What you're describing reminds me of this project: Vox/Farfissa organ
Have a look there to see if that helps.
Now you know that the hardware is working as it should, you could use a MIDI library, the one I use is here, but as you're only interested in Note on and off messages, it may be a bit over the top.
Slightly off topic, but there is a truly awesome DIY Hammond B3 project called PropB3 which you might be interested in.
It works very well.
I will, hopefully this week, give your code a go.
Regards
S