In a Cartesian coordinate system / Euclidian plane, a vehicle travels clockwise around the origin with radius 1 and currently at angle θ = PI/4 (45°), so its heading is 7/4*PI (315°).
To reach its new destination (the origin) it should change course (instantly) to -5/4*PI (-135°).
This angle can be obtained with e.g. atan2 on the components of the vector from position to target (with the target at origin, it's the the inverted position vector)b:
- Finding Signed Angle Between Vectors
- Using atan2 to find angle between two vectors
- Algorithm Improvement: Calculate signed change in heading
However, due to inertia, the vehicle cannot change its course instantly and crosses the X-axis (x=1, y=0, θ=0), where a new calculation results in a course of PI (180°).
The vehicle tries to achieve this new (positive angle) course by a positive turn (counterclockwise, CCW), which takes it back across the X-axis, and so on (it "wiggles" away from the target along the x-axis).
It breaks also if negative angles are "wrapped" into the absolute positive range 0...2PI - example data of the southbound vehicle passing west of the target:
course -0.00477
adjust 6.27840
course 0.00034
adjust 0.00034
The proportional control then basically causes a negative (clockwise) turn, which is the wrong direction.
Do I eliminate this "overflow" somehow (not limit to 0...2PI but map absolute angles to the vehicle's) or should I use a different strategy altogether (which)?
