# How to handle robot movement close to coordinates x=0 with atan2

I am working on controlling a robot to various points on an x,y grid. The robot starts at random locations and I can control it's velocity and angular velocity.

Currently, I am using the technique of rotate the robot until the atan2 of the distance between the robot and the target goal like so:

dx = target_position.x - robot_position.x
dy = target_position.y - robot_position.y
angle = atan2(dy, dx)
angle = wrap_to_pi(angle) # Wrap the angles to the range [-π, π] radians.

if angle - robot_angle <= some_error_factor:
increase_linear_velocity() # note this is pseudo code

This works pretty well to get me to target positions when the robot never has to go near x=0 on the coordinate system. I know there is a discontinuity with atan2 near pi, but my question is how can I wrap the angle or control the robot because currently when x is close to 0 the robot just spins around unable to get under the error factor to say you're pointing at the position

• The error on the angle shall have control on the angular velocity, not on the linear velocity. – Ugo Pattacini Feb 5 at 17:34
• Sorry it's not the full code. But basically it is set up that the robot will spin until the difference of the desired angle and the angle of the robot is lower than the tolerance and if it is stop rotating and apply a linear velocity – Graham Chapman Feb 5 at 17:51

First off, your code may have some confusion about atan2, which already returns an angle between -pi and +pi. That's one reason to use atan2(dy, dx) instead of atan(dy/dx) -- it figures out the correct quadrant so we don't have to. (The other reason to use atan2 is that it works fine when dx is near zero, where atan(dy/dx) will suffer loss of precision, overflow, or even division by zero.)

Secondly, I think your code is doing the pi-wrap in the wrong place. Try something more like this:

dx, dy = target_position - robot_position
target_angle = atan2(dy, dx)
d_angle = target_angle - robot_angle
d_angle = wrap_to_p(d_angle)    # wrap the angle here
if abs(d_angle) < acceptable_error