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