Changed integral to reset when desired heading changes