Don't hard code the threshold for the orientation error

This should prevent all cases of oscillation, since the most we ever command
the robot to turn is accel_lim_w the robot will oscillate around zero by at most
 accel_lim_w * period^2 / 2