Added servo_init() to main()
Added packet ids for servo commands
\#define USE_COMMANDS 1
Added skeleton for servo commands