Support servo command
Move constants to config.h