Improved calibration routine

Made calibration routine more robust so would run well on Teensy 3.1 as well as Pro Mini.