I moved constants up to the top and made the early tolerance 5ms and the late tolerance 100ms.