Added directorys for servo, and a new test platform.c