Added check to ensure start angle is correct - needs testing with robot