Cleaned up code, added comments, fixed some configuration and calibration things