Fixed staff code bug with compass.

Staff code used atan2 to get angle, but this returns values from (-pi,
pi] and the code treated it as if values were always non-negative. Fixed
by adding 2*pi to negative headings.