Improved calibration routine

Made calibration routine more robust so it would run without problems on both the Teensy 3.1 and the Pro Mini