Added test script for servo functionality