fixed #1. fixed #2. Also the program can now be compiled withouth servo support by undefining USE_SERVO