Added Servo Controller library + Unit tests