Added calibration routine, clean up