Remove hysteresis/2 and add hysteresis to PID