Quote:
Originally Posted by Strants
If you plan to use vectors, you could actually just use some properties of vectors to find the target angle. Let U be the vector in the direction the wheels are currently facing, and V be the vector in the direction you want the wheels to face. Assuming U and V are unit vectors (that is, they have a length of one), the angle between then (let's call it Q) is found by the equation Q = acos((Ux*Vx)+(Uy*Vy)). Because acos returns a value between 0 and pi, we can subtract Q from pi if Q is greater than pi/2 (because, if Q is greater than pi/2, it is quicker to turn the wheels to face in the opposite direction and rotate them backwards). After this set, Q is between 0 and pi/2, but we don't know which direction to rotate in. Assume a positive value of Q corresponds to a counterclockwise rotation, we can then give Q the same sign as the expression (Ux * Vy - Uy * Vx), which is always positive if the rotation required to move U to V is counterclockwise, and negative if the rotation is clockwise. This will give you the shortest counterclockwise rotation, with negative magnitude benefits.
|
If you don't use vectors, you could do the following:
Let U be the angle (in radians) in the direction the wheels are currently facing,
and V be the angle in the direction you want the wheels to face.
Assuming U and V are both measured in the same direction from the same zero reference,
the angle between them (let's call it Q) is found by the equation:
Q = (V-U) - 2pi*floor(0.5+(V-U)/2pi);
Because the above returns a value for Q between -pi and pi,
it tells you
which direction to rotate as well as
how much to rotate.
If you want to add logic to reverse direction,
that is straightforward to do at this point.
Be aware that
it is not necessarily true that steering angle changes
greater than pi/2 are best handled by reversing wheel speed direction.
It depends on many factors, such as the present value of wheel speed,
and the relative dynamic response of your wheel speed vs steering angle.