Update thresholds & goal alignment

Update thresholds & goal alignment strategy.
When rotating, move - stop - move - stop to align.