Final code for servo testing! 

use me!