Add Controller with user input
Rolling average of position instead of repeated reads w/ delay
Make debug interfere less with actual functions.