Added some parsing routines and CmdServo