Turn to point


I end up with my programming of odometry and PID. After this, I want to work so that my robot turns the heading to positions x and y, which I will implement. Unfortunately, I have no clue how to do it, so the formulas or example codes will be really helpful.


Take the tan of difference of y divided by the difference of x for the angle. I don’t think this will work in the third quadrant unless your fine with your robot’s back pointing toward the target. If your confused look up some videos explains trigonometry.

Have the positive x axis point in the direction of the front of the robot
Have the positive y axis point to the left of the robot

This ensures that when the robot starts at 0 degrees it is facing in the X direction which makes the math much easier.

Do deltaX= desiredX-robotX;
deltaY = desiredY-robotY;

Desired heading = atan2(deltaY,deltaX);

desired heading will be where the robot should point given in radians.

atan2 is used instead of atan because atan only returns angles from 0 to pi but atan2 consideres the signs of deltaY and deltaX so it knows where from -pi to pi the robot should point.

For documentation see here: atan2 - C++ Reference


Thanks, it is really helpful

However, it makes me another question.
How can I turn the robot to a specific angle?

ex. my robot is 0, and my desire is 270. So the robot calculates, that it is faster to turn left by 90 deg. I know how to calculate to which side it is faster and how much, but I don’t know how to transform it to the PID.


It would depends how your PID works but I would assume you would say if target angle is negative start turning to the left

just start with a P loop

Take how far away the robot’s heading is from where it should be and multiply that by a constant.

This makes it so when the bot is far it goes fast and slow when it goes close.