Changed distance controller to pid, major code cleanup
add HZ to 50
change HX back to 10
changed default values