PID crossing 0

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

In psuedo-C:

//To start, we assume magnitude is positive
bool neg_mag_benefits = false; 
double getCorrectionAngle(double currentAngle, double destinationAngle) {
  double currentX = cos(currentAngle);
  double currentY = sin (currentAngle);
  double destinationX = cos(destinationAngle);
  double destinationY = sin(destinationAngle);
  double correctionAngle = acos(currentX * destinationX + currentY * destinationY);
  //correctionAngle is in the range 0 to PI.
  double crossProd = currentX * destinationY - currentY * destinationX;
  if(correctionAngle > PI/2) {
    neg_mag_benefits = true; //We will be turning the wheels in a negative direction
    correctionAngle = PI - correctionAngle; //In range 0 to PI/2
  }
  correctionAngle *= abs(crossProd)/crossProd;
  return correctionAngle;
}