Initial crack at servo feedback, seems to work reasonably well
Refactored with some defines
Code cleanup