Arduino void loop stops after a while

问题:

/*******************************************************************************************************

I am currently building an autonomous vehicle.

What it does is as follows it draws GPS information and Magnetometer Sensor data.

Then when all sensor data is collected, Arduino decides which way to turn.

This is done by calculating a heading error.

*******************************************************************************************************/

/*******************************************************************************************************

My expectation:

When running, Arduino gathers sensor data.

Based on sensor data, arduino decides the direction to turn.

When signal is sent to motor driver, the arduino loops to collect the sensor data.

This has to be done relatively fast because only one wheel turning wheel result complete rotation of the vehicle.

*******************************************************************************************************/

When running separately, the Arduino can acquire Magnetometer Sensor and GPS info without stopping.

However, when the combined code is running, Arduino stops after awhile.

Furthermore, the response of the magnetometer seems to slow down as well when running the motor. /*******************************************************************************************************

To resolve the issue, I have return function in both to if statement and "SmallLeftTurn" function to attempt at breaking the what appeared to be stuck.

Below is the troubled code that blocks Arduino from functioning.

void calcDesiredTurn()
{
  //Calculate Where We Need To Turn to Head To Destination.
  headingError = targetHeading - currentHeading;

  //adjust for compass wrap
  if (headingError < -180)
    headingError += 360;
  if (headingError > 180)
    headingError -= 180;

  //Calculate Which Way To Turn To Intercept the target Heading.
  if (abs(headingError) <= HEADING_TOLERANCE)
  {
    Straight();
    return;
  }
  else if (headingError < 0)
  {
    SmallLeftTurn();
  }
  else if (headingError > 0)
  {
    SmallRightTurn();

  }
  else
  {
    Straight();
  }
}

void Straight()
{
  RightMotor.setSpeed(50);
  LeftMotor.setSpeed(-50);
}
void SmallLeftTurn()
{
  LeftMotor.setSpeed(0);
  RightMotor.setSpeed(50);
}

void SmallRightTurn()
{
  RightMotor.setSpeed(0);
  LeftMotor.setSpeed(-50);
}
void Stop()
{
  LeftMotor.setSpeed(0);
  RightMotor.setSpeed(0);
}

My void loop is constructed in this manner.

void loop() {
  // put your main code here, to run repeatedly:
  if (GPS.newNMEAreceived())
  {
    Serial.println("GPS NMEA Received");
    Serial.println(GPS.lastNMEA());
    if (GPS.parse(GPS.lastNMEA()))
      GPSDataProcess();
  }
  //navigate
  currentHeading = readCompass();
  Serial.println(currentHeading);
  calcDesiredTurn();
  delay(200);
}