Cleaned up servo control testing code