Turn to point

Hey,

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.

Thanks

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 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

2 Likes

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.

Thanks

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