Refactored code. Put it into subfolder as requested by the Arduino IDE.
Updated servo code.