Added servo control stuff. Still needs speed control work