Subtract angles before passing to PID