Actually Adding Unit tests for Servo Controller