Calibration!

Added calibration function. Stop now works without deceleration.