First commit of servo code

This commit contains the code to move the camera around.