Finished the unit tests for servo position