Added code and comments

- Added code to servocontrol.cpp/h
- Added comments about pin numbering in lcsdscreen.cpp