Well even though it's not working as well as you want it, it is working somewhat and that's significant progress! 
Panici:
Suggestions?
Post your current code in full, at least if you want meaningful assistance on it. 
For example, I'm not even sure if you are polling in the main loop, using a timer, or a pin change interrupt...