Clean up and added stuff

- Changed from printf to cout.
- Added function to load servo values from ini-file