I'm applying a PID for position control on a diff drive robot, got the angles (orientation )between -pi and pi. It works fine until it needs to change its orientation from (let's say) -170 degrees to 170, when instead of changing its orientation by only 20 degrees it does an entire turn (almost 360) to get to the desired orientation. Does anyone knows how to solve this?
This looks like a common angle wrapping mistake. I am assuming you're defining your angles between -180° ($-\pi$) to +180° ($\pi$).
Let's say your current heading is -170°.
And you desired heading is 170°.
The error in angle is: 170 - (-170) = 340, so your robot has to do almost a full turn to get to the desired heading.
Since you're defining your angle in the range $\left[ -180 ; 180 \right] $, you should also wrap the error in the same range. In the example above, this means removing 360, so the error becomes 340 - 360 = -20. Which is what your PID needs to correct the heading.