Refactor files

[AndunHH]

This pull request has refactoring and cleans the code without real change in functionality!

• Updated Readme with more details and some better overview. Stil needs more explanation!
• Refactored translational and rotational variables into a "velocity" array for easier debug function calls.
• Changed many configs from variables to #defines.
• Replaced magic numbers in the code with config #defines.
• Moved all the calibration outputs to calibration.h and .cpp for better overview and refactored them to save some memory.