Added the beginnings of the servo control