Little problem with my Stabilization Program - MSP

Using Arduino Programming Questions
1

Hello everybody,
I have I little problem with this code. The idea is that the robot before to go straight he reads data from compass. And when he go straight, he reads every second compass data again compares it, and if is it different, he corrects his drive direction.
So the code looks something like this:
maindirection = readcompas;
goforward;
currentdirection = readcompas;
if (currentdirection > maindirection) {
turnleft();
}
if(currentdirection < maindirection) {
turnright();
}

This code is working well if main direction is from about 50 to 310 degrees. Because if main direction is 360 or 1 he makes one turn to correct drive direction. Because if main direction = 358 and current direction = 2, he need only to turn left for 4 degrees and he will be on the right way.., but with my code he will turn right till 2 becomes 358(2,3,4,5,6,7,8,9..) so he will make one turn..
Maybe someone knows the way how to avoid this problem?
Thank you very much for your answers!
P.S. Why MSP and not ESP? Because: Magnetic Stabilization Program..:slight_smile: and not Electronic Stabilization Program.. like on VW...:slight_smile:

2

Subtract desired heading from current heading to get the angle you want to turn.
Use modulus to convert this to a number between -180 ... 180.
Turn by the angle you have calculated.

3

Cruder version of PeterH's solution, but since I had typed it up, here it is anyway:

if (currentdirection > maindirection) 
  {
  if((currentdirection-maindirection) < 180)
    turnleft();
  else
    turnright();
  }
else 
  {
  if((maindirection-currentdirection) < 180)
    turnright();
  else
    turnleft();
  }
4

Thank you wildbil and PeterH for great answer!