Actually wasn't that hard.....(I think)

Try this out, as it would take me some time to get an encoder setup to test.

https://dl.dropboxusercontent.com/s/...q4u2v8miy/sbpd

Let me know how this works...... if needed I will setup an encoder.