Arduino Mega 2560 hangs when calling compass

Hi, I am having an issue in my arduino mega 2560 which is connected to a duinotech 5883L magnometer and duinotech gps unit. The code runs fine until i call the go waypoint function, which executes its loop several times then freezes when calling the compass when it tries to execute a compass.readnormalize function.

I am using the compass library

and the following is my code from a rover project

'''
#include "Wire.h"                                                 // Used by I2C and HMC5883L compass

// #include "I2Cdev.h"                                             // I2C Communications Library (used for compass)

//#include "I2C.h"

#include "HMC5883L.h"                                             // Library for the compass - Download from Github @ https://github.com/jarzebski/Arduino-HMC5883L

                                              // Servo library to control Servo arm for metal detector

#include <SoftwareSerial.h>                                       // Software Serial for Serial Communications - not used

#include <TinyGPS++.h>                                            // Tiny GPS Plus Library - Download from http://arduiniana.org/libraries/tinygpsplus/

                                                                  // TinyGPS++ object uses Hardware Serial 2 and assumes that you have a

                                                                  // 9600-baud serial GPS device hooked up on pins 16(tx) and 17(rx).  

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

// Digital Motor Output Setup

int leftMotor = 42;

int rightMotor = 43;

                                                                                                                                 

// GPS Variables & Setup//******************************************************************************************************                                                                  

int GPS_Course;                                                    // variable to hold the gps's determined course to destination

int Number_of_SATS;                                                // variable to hold the number of satellites acquired

TinyGPSPlus gps;                                                   // gps = instance of TinyGPS

                                                                   // pin 17 (blue)   is connected to the TX on the GPS

                                                                   // pin 16 (yellow) is connected to the RX on the GPS

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

                                           // motor speed when moving forward and reverse

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

// Compass Variables & Setup

HMC5883L compass;                                                  // HMC5883L compass(HMC5883L)

int16_t mx, my, mz;                                                // variables to store x,y,z axis from compass (HMC5883L)

int desired_heading;                                               // initialize variable - stores value for the new desired heading

int compass_heading;                                               // initialize variable - stores value calculated from compass readings

int compass_dev = 15;                                               // the amount of deviation that is allowed in the compass heading - Adjust as Needed

                                                                   // setting this variable too low will cause the robot to continuously pivot left and right

                                                                   // setting this variable too high will cause the robot to veer off course

int Heading_A;                                                     // variable to store compass heading

int Heading_B;                                                     // variable to store compass heading in Opposite direction

int pass = 0;                                                      // variable to store which pass the robot is on

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

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

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

// Bluetooth Variables & Setup

int indx;                                                           // command buffer

String str;                                                        // raw string received from android to arduino

int blueToothVal;                                                  // stores the last value sent over via bluetooth

                                                   // Pin 34 of the Aruino Mega used to test the Bluetooth connection status - Not Used

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

// GPS Locations

unsigned long Distance_To_Home;                                    // variable for storing the distance to destination

int ac =0;                                                         // GPS array counter

int wpCount = 0;                                                   // GPS waypoint counter

double Home_LATarray[6];                                          // variable for storing the destination Latitude - Only Programmed for 5 waypoint

double Home_LONarray[6];                                          // variable for storing the destination Longitude - up to 50 waypoints

int increment = 0;

void setup()

{  

  Serial.begin(115200);                                            // Serial 0 is for communication with the computer and magnometer.

  // Serial1.begin(11520);                                             // Serial 1 is for Bluetooth communication - DO NOT MODIFY - JY-MCU HC-010 v1.40

  Serial2.begin(9600);                                             // Serial 2 is for GPS communication at 9600 baud - DO NOT MODIFY - Ublox Neo 6m

 

  // Motors

  pinMode(leftMotor, OUTPUT);

  pinMode(rightMotor, OUTPUT);

  // bluetooth buffer

  indx = 0;

  // Compass

  while (!compass.begin())

  {                      

    Wire.begin();                             // Join I2C bus used for the HMC5883L compass

    compass.begin();                                                 // initialize the compass (HMC5883L)

    compass.setRange(HMC5883L_RANGE_1_3GA);                          // Set measurement range  

    compass.setMeasurementMode(HMC5883L_CONTINOUS);                  // Set measurement mode  

    compass.setDataRate(HMC5883L_DATARATE_15HZ);                     // Set data rate  

    compass.setSamples(HMC5883L_SAMPLES_8);                          // Set number of samples averaged  

    compass.setOffset(65,-290);                                          // Set calibration offset

  }

  Startup();                                                       // Run the Startup procedure on power-up one time

}

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

// Main Loop

void loop()

{

 

  bluetooth();  

  delay(10); // Run the Bluetooth procedure to see if there is any data being sent via BT                                                    

  getGPS();  

  delay(10); // Update the GPS location

  getCompass();

  Serial.println(compass_heading);

  delay(10);

  // Update the Compass Heading

  // Not in use... need servo and rangefinder to complete. Ping();                                                          // Use at your own discretion, this is not fully tested

}

// Functionality of Bluetooth

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

// This procedure reads the serial port - Serial1 - for bluetooth commands being sent from the Android device

void bluetooth()

{

 while (Serial.available()>0)                                    // Read bluetooth commands over Serial1 // Warning: If an error with Serial1 occurs, make sure Arduino Mega 2560 is Selected

 {  

   

    str = Serial.readStringUntil('0');                      // str is the temporary variable for storing the last sring sent over bluetooth from Android device

    Serial.print(str);                                      // for testing purposes

 

    blueToothVal = str.toInt();                               //  convert the string 'str' into an integer and assign it to blueToothVal

    Serial.print("Input Value: ");

    Serial.println(blueToothVal);  

    delay(60);                                                // delay to clear buffer

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

 switch (blueToothVal)

 {

      case 1:                                

        Serial.println(F("Forward"));

        Forward();

        break;

      case 2:  

        Serial.println(F("Left"));

        LeftTurn();

        StopCar();      

        break;

       

      case 3:                    

        Serial.println(F("Right"));

        RightTurn();

        StopCar();

        break;

       

      case 4:                                            

        Serial.println(F("Stop Car "));

        StopCar();

        break;

      case 5:                

        setWaypoint();

        break;

     

      case 6:        

        goWaypoint();

        break;  

      case 7:

        Serial.println(F("GPS Info"));

        gpsInfo();

        break;

     

//      case 15:  

//        pingToggle();

//        break;  

      case 8:

        clearWaypoints();

        break;  

      case 9:                    // finish with waypoints

        ac = 0;

        Serial.print(F("Waypoints Complete"));

        break;

      default:

        Serial.println(F("Invalid Command"));

        break;

     

 }

 }                                                              // end of while loop Serial1 read

 

                                                               // if no data from Bluetooth

   if (Serial.available() < 0)                                 // if an error occurs, confirm that the arduino mega board is selected in the Tools Menu

    {

     Serial.println(F("No Bluetooth Data "));          

    }

 

} // end of bluetooth procedure

// Going to waypoint Code

void goWaypoint()

{  

 Serial.println(F("Go to Waypoint"));

 ac=ac-1;

 while (true)  

  {                                                                // Start of Go_Home procedure

  // bluetooth();                                                     // Run the Bluetooth procedure to see if there is any data being sent via BT

  // if (blueToothVal == 4)

  //   {

  //   Serial.println("Bluetooth Break");

  //   break;

  //   }                                   // If a 'Stop' Bluetooth command is received then break from the Loop

  if (ac==-1){

    break;

  }

  Serial.println(F("Checking Compass"));

  getCompass();  

  delay(80);// Update Compass heading  

  Serial.println(F("Checking GPS"));                                        

  getGPS();

  delay(20);

  // Tiny GPS function that retrieves GPS data - update GPS location// delay time changed from 100 to 10

 

  if (millis() > 5000 && gps.charsProcessed() < 10)

    {               // If no Data from GPS within 5 seconds then send error

    Serial.println(F("No GPS data: check wiring"));    

    }

  Serial.println(F("Calculating"));

  Distance_To_Home = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location .lng(),Home_LATarray[ac], Home_LONarray[ac]);  //Query Tiny GPS for Distance to Destination

  GPS_Course = TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),Home_LATarray[ac],Home_LONarray[ac]);                               //Query Tiny GPS for Course to Destination  

  Serial.println(F("The Distance to waypoint: "));

  Serial.println(Distance_To_Home);

  Serial.println(F("The GPS Course Needed: "));

  Serial.println(GPS_Course);

   

  if (Distance_To_Home <= 1.5)                                   // If the Vehicle has reached it's Destination, then Stop

        {

        StopCar();                                               // Stop the robot after each waypoint is reached

        Serial.println(F("You have arrived!"));                    // Print to Bluetooth device - "You have arrived"          

        ac--;                                                    // increment counter for next waypoint

        if (ac==-1) {

          break;

          }                                                   // Break from Go_Home procedure and send control back to the Void Loop                                                                  // go to next waypoin      

        }  

   else if ( abs(GPS_Course - compass_heading) <= 30)                  // If GPS Course and the Compass Heading are within x degrees of each other then go Forward                                                                                                                                    

       {                                                          // otherwise find the shortest turn radius and turn left or right

         Forward();                                               // Go Forward

       } else

         {                                                      

            int x = (GPS_Course - 360);                           // x = the GPS desired heading - 360

            int y = (compass_heading - (x));                      // y = the Compass heading - x

            int z = (y - 360);                                    // z = y - 360

           

            if ((z <= 180) && (z >= 0))                           // if z is less than 180 and not a negative value then turn left otherwise turn right

                  {

                    Serial.println(F("Turning Left:"));

                    SlowLeftTurn();

                      }

             else

             {

               Serial.println(F("Turning Right"));

               SlowRightTurn();

             }              

          }

  }                                                              // End of While Loop

}                                                                // End of Go_Home procedure

 

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

void getGPS()                                                 // Get Latest GPS coordinates

{

    while (Serial2.available() > 0)

    gps.encode(Serial2.read());

    delay(60);

}

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

 

void setWaypoint()                                            // Set up to 5 GPS waypoints

{

//if ((wpCount >= 0) && (wpCount < 50))

if (wpCount >= 0)

  {

    Serial.print("GPS Waypoint ");

    Serial.print(wpCount + 1);

    Serial.print(" Set ");

    getGPS();                                                 // get the latest GPS coordinates

    getCompass();                                             // update latest compass heading    

                                               

    Home_LATarray[ac] = gps.location.lat();                   // store waypoint in an array  

    Home_LONarray[ac] = gps.location.lng();                   // store waypoint in an array  

                                                             

    Serial.print("Waypoint #1: ");

    Serial.print(Home_LATarray[0],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[0],6);

    Serial.print("Waypoint #2: ");

    Serial.print(Home_LATarray[1],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[1],6);

    Serial.print("Waypoint #3: ");

    Serial.print(Home_LATarray[2],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[2],6);

    Serial.print("Waypoint #4: ");

    Serial.print(Home_LATarray[3],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[3],6);

    Serial.print("Waypoint #5: ");

    Serial.print(Home_LATarray[4],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[4],6);

    wpCount++;                                                  // increment waypoint counter

    ac++;                                                       // increment array counter

       

  }        

  else {Serial.print("Waypoints Full");}

}

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

void clearWaypoints()

{

   memset(Home_LATarray, 0, sizeof(Home_LATarray));             // clear the array

   memset(Home_LONarray, 0, sizeof(Home_LONarray));             // clear the array

   wpCount = 0;                                                 // reset increment counter to 0

   ac = 0;

   

   Serial.println("GPS Waypoints Cleared");                      // display waypoints cleared

 

}

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

 

void getCompass()                                               // get latest compass value

 {  

  Serial.println(F("In getCompass"));

  Vector norm = compass.readNormalize();

  float declinationAngle = 0.418;

  // Calculate heading

  Serial.println(F("Before Atan"));

  float heading = atan2(norm.YAxis, norm.XAxis);

  heading += declinationAngle;

  // heading = heading-0.4;

  Serial.println(F("Positive Balance"));

  if(heading < 0)

     heading += 2 * M_PI;  

  Serial.println(F("Radians to degrees"));    

  compass_heading = (int)(heading * 180/M_PI);  

  delay(100);                 // assign compass calculation to variable (compass_heading) and convert to integer to remove decimal places                                                              

  Serial.println(F("Get Compass Finished"));

 }

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

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

 

void gpsInfo()                                                  // displays Satellite data to user

  {

        Number_of_SATS = (int)(gps.satellites.value());         //Query Tiny GPS for the number of Satellites Acquired

        Distance_To_Home = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),Home_LATarray[ac], Home_LONarray[ac]);  //Query Tiny GPS for Distance to Destination    

        Serial.print("Lat:");

        Serial.print(gps.location.lat(),6);

        Serial.print(" Lon:");

        Serial.print(gps.location.lng(),6);

        Serial.print(" ");

        Serial.print(Number_of_SATS);

        Serial.print(" SATs ");

       

        Serial.print(Distance_To_Home);

        Serial.print("m");

Serial.print("Distance to Home ");

Serial.println(Distance_To_Home);

 

  }

  // Start-up function to be run inside of set-up and to initialise the rover for use

void Startup()

{

                  

     for (int i=5; i >= 1; i--)                       // Count down for X seconds

      {        

        Serial.print("Pause for Startup... ");

        Serial.print(i);

        delay(1000);                                   // Delay for X seconds

      }    

     

  Serial.println("Searching for Satellites ");

     

  while (Number_of_SATS < 0)                         // Wait until x number of satellites are acquired before starting main loop

  {              

    Serial.println("Small delay for home co-ordinates to be recieved");

    delay(3000);                    

    getGPS();                                         // Update gps data

    Serial.println(int(gps.satellites.value()));

    Number_of_SATS = (int)(gps.satellites.value());   // Query Tiny GPS for the number of Satellites Acquired      

    bluetooth();                                      // Check to see if there are any bluetooth commands being received  

    Serial.println("Looping for SATS");

    gpsInfo();

  }                                        // set intial waypoint to current location

  wpCount = 0;                                        // zero waypoint counter

  ac = 0;                                             // zero array counter

   

  Serial.print(Number_of_SATS);

  Serial.print(" Satellites Acquired");

  Serial.println(" Start-up Complete");  

}        

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

// This Section Deals with the steering of the rover

void Forward()

{

//  Ping();

  Serial.println("Forward");

  // motor1.setSpeed(mtr_Spd);                                                  

  // motor2.setSpeed(mtr_Spd);    

  // motor1.run(FORWARD);                                                         // go forward all wheels

  // motor2.run(FORWARD);

  digitalWrite(leftMotor, HIGH);

  digitalWrite(rightMotor, HIGH);

  // delay(1000);

  // digitalWrite(leftMotor, LOW);

  // digitalWrite(rightMotor, LOW);

 

}

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

void Forward_Meter()

{

  // motor1.setSpeed(mtr_Spd);                                                  

  // motor2.setSpeed(mtr_Spd);    

  // motor1.run(FORWARD);                                                        // go forward all wheels for specified time

  // motor2.run(FORWARD);

  digitalWrite(leftMotor, LOW);

  digitalWrite(rightMotor, LOW);

  delay(20);

  digitalWrite(leftMotor, HIGH);

  digitalWrite(rightMotor, HIGH);

  delay(3000);

  digitalWrite(leftMotor, LOW);

  digitalWrite(rightMotor, LOW);

}

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

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

void LeftTurn()

{

  // motor1.setSpeed(mtr_Spd);                                                  

  // motor2.setSpeed(mtr_Spd);    

  //                                                       // Turn left

  // motor2.run(FORWARD);

  digitalWrite(rightMotor, HIGH);

  digitalWrite(leftMotor, LOW);

  delay(3000);    

  digitalWrite(rightMotor, LOW);

}

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

void RightTurn()

{

  // motor1.setSpeed(mtr_Spd);                                                  

  // motor2.setSpeed(mtr_Spd);    

   

  // motor1.run(FORWARD);                                              

  digitalWrite(leftMotor, HIGH);

  digitalWrite(rightMotor, LOW);

  delay(3000);    

  digitalWrite(leftMotor, LOW);                                                                  //delay for 100ms more responsive turn per push on bluetooth  

}

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

void SlowLeftTurn()

{

   

  // motor1.setSpeed(turn_Speed);                                                

  // motor2.setSpeed(turn_Speed);                      

  digitalWrite(rightMotor, LOW);

  digitalWrite(leftMotor,LOW);

  delay(20);

 

  digitalWrite(rightMotor, HIGH);

                           

  // motor2.run(FORWARD);

 

}

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

// Turning too fast will over-shoot the compass and GPS course

void SlowRightTurn()

{

   

  // motor1.setSpeed(turn_Speed);                                                  

  // motor2.setSpeed(turn_Speed);                      

  digitalWrite(rightMotor, LOW);

  digitalWrite(leftMotor,LOW);

  delay(20);

  // motor1.run(FORWARD);                                                          

  digitalWrite(leftMotor, HIGH);

       

}

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

void StopCar()

{

    

  // motor1.run(RELEASE);                                                        

  // motor2.run(RELEASE);

  digitalWrite(leftMotor, LOW);

  digitalWrite(rightMotor, LOW);

   

}
'''

and finally the error screen when the gowaypoint() function is called

The gowaypoint() function executes four times and then freezes on the line
vector norm = compass.readNormalize();

The nature of this error makes me believe it may be some kind of memory leak but I am at the beginning of the learning curve with arduino and low level programming so would love any help that the forum could provide.

EDIT: To clarify the compass and gps work fine when they are called in the main loop, it is only when called by the go waypoint function in the while loop that the error occurs which leads me to believe it is some kind of memory leak issue?

A 'while(1)' forever loop with only breaks for exit is a dangerous game. It makes it hard to see what the logic leading up to an exit condition is. You may just be stuck in the loop. You could add a debug statement in the outer part of the loop to test that.

agree, however that isn't my issue at the moment. I am trying to stay in that loop to test that it can operate for long periods, and to that end it isn't. The code should only escape when the last waypoint is reached, I do plan to expand on escape conditions later once i get the basic code in a working state.

You say it freezes here. How do you know that?

I have a Serial.println("In getCompass"); in the proceeding line of the getCompass() function which is where the execution freezes in the serial port output. It never reaches the next serial.print in that function.

'''
void getCompass()                                               // get latest compass value

 {  

  Serial.println(F("In getCompass"));

  Vector norm = compass.readNormalize();

  float declinationAngle = 0.418;

  // Calculate heading

  Serial.println(F("Before Atan"));

  float heading = atan2(norm.YAxis, norm.XAxis);

  heading += declinationAngle;

  // heading = heading-0.4;

  Serial.println(F("Positive Balance"));

  if(heading < 0)

     heading += 2 * M_PI;  

  Serial.println(F("Radians to degrees"));    

  compass_heading = (int)(heading * 180/M_PI);  

  delay(100);                 // assign compass calculation to variable (compass_heading) and convert to integer to remove decimal places                                                              

  Serial.println(F("Get Compass Finished"));

 }
'''

Yes, but there is code between the line in question, and the next debug statement...

Ok, I'll add another print line between the float declaration and the vector. That is my bad, I thought it was a pretty safe bet it wasn't the float declaration, maybe my inexperience showing.

It's better to leave nothing to chance. I see your point, I also doubt the following line should have any effect. Is it possible to comment out the line? Or substitute a dummy value?

Also, could you please give us a complete text listing of the output, instead of a screen shot?

You should probably print out these values too. Maybe you hit a divide by zero error. A guess.

Also, does it run exactly the same number of times each run, before crashing?

Have you disabled interrupts anywhere? Has any library done so?

...and... sorry to pile it on... but as many other people will examine your code, could you repost it without the annoying double line spacing?

Okay, will make the changes and run them on the prototype shortly. And yes always crashes at the same point after 4 executions, currently... although it did run longer before I started debugging it(adding more code to the loop and delays) which led me to believe it was a memory leak of some kind(it did appear to crash at the same point of the loop though), although the mega should have plenty of ram for the code.

Have you tested all the hardware independently, using only simple example sketches instead of this one?

Yes gps and compass were both run seperately and operated fine and were both tested on a uno as well.

I haven't messed with interupts.

'''
#include "Wire.h"                                                 // Used by I2C and HMC5883L compass

// #include "I2Cdev.h"                                             // I2C Communications Library (used for compass)

//#include "I2C.h"

#include "HMC5883L.h"                                             // Library for the compass - Download from Github @ https://github.com/jarzebski/Arduino-HMC5883L

                                              // Servo library to control Servo arm for metal detector

#include <SoftwareSerial.h>                                       // Software Serial for Serial Communications - not used

#include <TinyGPS++.h>                                            // Tiny GPS Plus Library - Download from http://arduiniana.org/libraries/tinygpsplus/

                                                                  // TinyGPS++ object uses Hardware Serial 2 and assumes that you have a

                                                                  // 9600-baud serial GPS device hooked up on pins 16(tx) and 17(rx).  

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

// Digital Motor Output Setup

int leftMotor = 42;

int rightMotor = 43;

                                                                                                                                 

// GPS Variables & Setup//******************************************************************************************************                                                                  

int GPS_Course;                                                    // variable to hold the gps's determined course to destination

int Number_of_SATS;                                                // variable to hold the number of satellites acquired

TinyGPSPlus gps;                                                   // gps = instance of TinyGPS

                                                                   // pin 17 (blue)   is connected to the TX on the GPS

                                                                   // pin 16 (yellow) is connected to the RX on the GPS

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

                                           // motor speed when moving forward and reverse

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

// Compass Variables & Setup

HMC5883L compass;                                                  // HMC5883L compass(HMC5883L)

int16_t mx, my, mz;                                                // variables to store x,y,z axis from compass (HMC5883L)

int desired_heading;                                               // initialize variable - stores value for the new desired heading

int compass_heading;                                               // initialize variable - stores value calculated from compass readings

int compass_dev = 15;                                               // the amount of deviation that is allowed in the compass heading - Adjust as Needed

                                                                   // setting this variable too low will cause the robot to continuously pivot left and right

                                                                   // setting this variable too high will cause the robot to veer off course

int Heading_A;                                                     // variable to store compass heading

int Heading_B;                                                     // variable to store compass heading in Opposite direction

int pass = 0;                                                      // variable to store which pass the robot is on

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

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

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

// Bluetooth Variables & Setup

int indx;                                                           // command buffer

String str;                                                        // raw string received from android to arduino

int blueToothVal;                                                  // stores the last value sent over via bluetooth

                                                   // Pin 34 of the Aruino Mega used to test the Bluetooth connection status - Not Used

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

// GPS Locations

unsigned long Distance_To_Home;                                    // variable for storing the distance to destination

int ac =0;                                                         // GPS array counter

int wpCount = 0;                                                   // GPS waypoint counter

double Home_LATarray[6];                                          // variable for storing the destination Latitude - Only Programmed for 5 waypoint

double Home_LONarray[6];                                          // variable for storing the destination Longitude - up to 50 waypoints

int increment = 0;

void setup()

{  

  Serial.begin(115200);                                            // Serial 0 is for communication with the computer and magnometer.

  // Serial1.begin(11520);                                             // Serial 1 is for Bluetooth communication - DO NOT MODIFY - JY-MCU HC-010 v1.40

  Serial2.begin(9600);                                             // Serial 2 is for GPS communication at 9600 baud - DO NOT MODIFY - Ublox Neo 6m

  pinMode(leftMotor, OUTPUT);

  pinMode(rightMotor, OUTPUT);

  indx = 0;

  // Compass

  while (!compass.begin())

  {                      

    Wire.begin();                             // Join I2C bus used for the HMC5883L compass

    compass.begin();                                                 // initialize the compass (HMC5883L)

    compass.setRange(HMC5883L_RANGE_1_3GA);                          // Set measurement range  

    compass.setMeasurementMode(HMC5883L_CONTINOUS);                  // Set measurement mode  

    compass.setDataRate(HMC5883L_DATARATE_15HZ);                     // Set data rate  

    compass.setSamples(HMC5883L_SAMPLES_8);                          // Set number of samples averaged  

    compass.setOffset(65,-290);                                          // Set calibration offset

  }

  Startup();                                                       // Run the Startup procedure on power-up one time

}

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

// Main Loop

void loop()

{

 

  bluetooth();  

  delay(10); // Run the Bluetooth procedure to see if there is any data being sent via BT                                                    

  getGPS();  

  delay(10); // Update the GPS location

  getCompass();

  Serial.println(compass_heading);

  delay(10);

  // Update the Compass Heading

  // Not in use... need servo and rangefinder to complete. Ping();                                                          // Use at your own discretion, this is not fully tested

}

// Functionality of Bluetooth

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

// This procedure reads the serial port - Serial1 - for bluetooth commands being sent from the Android device

void bluetooth()

{

 while (Serial.available()>0)                                    // Read bluetooth commands over Serial1 // Warning: If an error with Serial1 occurs, make sure Arduino Mega 2560 is Selected

 {  

    str = Serial.readStringUntil('0');                      // str is the temporary variable for storing the last sring sent over bluetooth from Android device

    Serial.print(str);                                      // for testing purposes

    blueToothVal = str.toInt();                               //  convert the string 'str' into an integer and assign it to blueToothVal

    Serial.print("Input Value: ");

    Serial.println(blueToothVal);  

    delay(60);                                                // delay to clear buffer

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

 switch (blueToothVal)

 {

      case 1:                                

        Serial.println(F("Forward"));

        Forward();

        break;

      case 2:  

        Serial.println(F("Left"));

        LeftTurn();

        StopCar();      

        break;

       

      case 3:                    

        Serial.println(F("Right"));

        RightTurn();

        StopCar();

        break;

       

      case 4:                                            

        Serial.println(F("Stop Car "));

        StopCar();

        break;

      case 5:                

        setWaypoint();

        break;

     

      case 6:        

        goWaypoint();

        break;  

      case 7:

        Serial.println(F("GPS Info"));

        gpsInfo();

        break;

      case 8:

        clearWaypoints();

        break;  

      case 9:                    // finish with waypoints

        ac = 0;

        Serial.print(F("Waypoints Complete"));

        break;

      default:

        Serial.println(F("Invalid Command"));

        break;

 }

 }                                                              // end of while loop Serial1 read

 

                                                               // if no data from Bluetooth

   if (Serial.available() < 0)                                 // if an error occurs, confirm that the arduino mega board is selected in the Tools Menu

    {

     Serial.println(F("No Bluetooth Data "));          

    }

 

} // end of bluetooth procedure

// Going to waypoint Code

void goWaypoint()

{  

 Serial1.println("Go to Waypoint");

//Serial.print("Home_Latarray ");

//Serial.print(Home_LATarray[ac],6);

//Serial.print(" ");

//Serial.println(Home_LONarray[ac],6);  

//Serial1.print("Distance to Home");  

//Serial1.print(Distance_To_Home);

//Serial1.print("ac= ");

//Serial1.print(ac);

 while (true)  

  {                                                                // Start of Go_Home procedure

  bluetooth();                                                     // Run the Bluetooth procedure to see if there is any data being sent via BT

  if (blueToothVal == 5){break;}                                   // If a 'Stop' Bluetooth command is received then break from the Loop

  getCompass();                                                    // Update Compass heading                                          

  getGPS();                                                        // Tiny GPS function that retrieves GPS data - update GPS location// delay time changed from 100 to 10

 

  if (millis() > 5000 && gps.charsProcessed() < 10)                // If no Data from GPS within 5 seconds then send error

    Serial1.println(F("No GPS data: check wiring"));    

 

  Distance_To_Home = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),Home_LATarray[ac], Home_LONarray[ac]);  //Query Tiny GPS for Distance to Destination

  GPS_Course = TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),Home_LATarray[ac],Home_LONarray[ac]);                               //Query Tiny GPS for Course to Destination  

   /*

    if (Home_LATarray[ac] == 0) {

      Serial1.print("End of Waypoints");

      StopCar();      

      break;

      }      

   */

    if (Distance_To_Home == 0)                                   // If the Vehicle has reached it's Destination, then Stop

        {

        StopCar();                                               // Stop the robot after each waypoint is reached

        Serial1.println("You have arrived!");                    // Print to Bluetooth device - "You have arrived"          

        ac++;                                                    // increment counter for next waypoint

        break;                                                   // Break from Go_Home procedure and send control back to the Void Loop

                                                                 // go to next waypoint

       

        }  

   if ( abs(GPS_Course - compass_heading) <= 15)                  // If GPS Course and the Compass Heading are within x degrees of each other then go Forward                                                                  

                                                                  // otherwise find the shortest turn radius and turn left or right  

       {

         Forward();                                               // Go Forward

       } else

         {                                                      

            int x = (GPS_Course - 360);                           // x = the GPS desired heading - 360

            int y = (compass_heading - (x));                      // y = the Compass heading - x

            int z = (y - 360);                                    // z = y - 360

           

            if ((z <= 180) && (z >= 0))                           // if z is less than 180 and not a negative value then turn left otherwise turn right

                  { SlowLeftTurn();  }

             else { SlowRightTurn(); }              

        }

   

  }                                                              // End of While Loop

}                                                                               // End of Go_Home procedure

 

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

void getGPS()                                                 // Get Latest GPS coordinates

{

    while (Serial2.available() > 0)

    gps.encode(Serial2.read());

    delay(60);

}

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

 

void setWaypoint()                                            // Set up to 5 GPS waypoints

{

//if ((wpCount >= 0) && (wpCount < 50))

if (wpCount >= 0)

  {

    Serial.print("GPS Waypoint ");

    Serial.print(wpCount + 1);

    Serial.print(" Set ");

    getGPS();                                                 // get the latest GPS coordinates

    getCompass();                                             // update latest compass heading                                                

    Home_LATarray[ac] = gps.location.lat();                   // store waypoint in an array  

    Home_LONarray[ac] = gps.location.lng();                   // store waypoint in an array                                                            

    Serial.print("Waypoint #1: ");

    Serial.print(Home_LATarray[0],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[0],6);

    Serial.print("Waypoint #2: ");

    Serial.print(Home_LATarray[1],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[1],6);

    Serial.print("Waypoint #3: ");

    Serial.print(Home_LATarray[2],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[2],6);

    Serial.print("Waypoint #4: ");

    Serial.print(Home_LATarray[3],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[3],6);

    Serial.print("Waypoint #5: ");

    Serial.print(Home_LATarray[4],6);

    Serial.print(" ");

    Serial.println(Home_LONarray[4],6);

    wpCount++;                                                  // increment waypoint counter

    ac++;                                                       // increment array counter  

  }        

  else {Serial.print("Waypoints Full");}

}

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

void clearWaypoints()

{

   memset(Home_LATarray, 0, sizeof(Home_LATarray));             // clear the array

   memset(Home_LONarray, 0, sizeof(Home_LONarray));             // clear the array

   wpCount = 0;                                                 // reset increment counter to 0

   ac = 0;

   Serial.println("GPS Waypoints Cleared");                      // display waypoints cleared

}

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

 

void getCompass()                                               // get latest compass value

 {  

  Serial.println(F("In getCompass"));

  Vector norm = compass.readNormalize();

  Serial.println(F("Vector Done Float Next"));

  // float declinationAngle = 0.418;

  // Calculate heading

  Serial.println(F("Before Atan"));

  float heading = atan2(norm.YAxis, norm.XAxis);

  Serial.println(F("Values in Atan"));

  Serial.println(norm.YAxis);

  Serial.println(norm.yAxis);

  // heading += declinationAngle;

  // heading = heading-0.4;

  Serial.println(F("Positive Balance"));

  if(heading < 0)

     heading += 2 * M_PI;  

  Serial.println(F("Radians to degrees"));    

  compass_heading = (int)(heading * 180/M_PI);  

  delay(100);                 // assign compass calculation to variable (compass_heading) and convert to integer to remove decimal places                                                              

  Serial.println(F("Get Compass Finished"));

 }

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

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

 

void gpsInfo()                                                  // displays Satellite data to user

  {

        Number_of_SATS = (int)(gps.satellites.value());         //Query Tiny GPS for the number of Satellites Acquired

        Distance_To_Home = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),Home_LATarray[ac], Home_LONarray[ac]);  //Query Tiny GPS for Distance to Destination    

        Serial.print("Lat:");

        Serial.print(gps.location.lat(),6);

        Serial.print(" Lon:");

        Serial.print(gps.location.lng(),6);

        Serial.print(" ");

        Serial.print(Number_of_SATS);

        Serial.print(" SATs ");

        Serial.print(Distance_To_Home);

        Serial.print("m");

Serial.print("Distance to Home ");

Serial.println(Distance_To_Home);

 

  }

  // Start-up function to be run inside of set-up and to initialise the rover for use

void Startup()

{      

     for (int i=5; i >= 1; i--)                       // Count down for X seconds

      {        

        Serial.print("Pause for Startup... ");

        Serial.print(i);

        delay(1000);                                   // Delay for X seconds

      }    

  Serial.println("Searching for Satellites ");

     

  while (Number_of_SATS < 0)                         // Wait until x number of satellites are acquired before starting main loop

  {              

    Serial.println("Small delay for home co-ordinates to be recieved");

    delay(3000);                    

    getGPS();                                         // Update gps data

    Serial.println(int(gps.satellites.value()));

    Number_of_SATS = (int)(gps.satellites.value());   // Query Tiny GPS for the number of Satellites Acquired      

    bluetooth();                                      // Check to see if there are any bluetooth commands being received  

    Serial.println("Looping for SATS");

    gpsInfo();

  }                                        // set intial waypoint to current location

  wpCount = 0;                                        // zero waypoint counter

  ac = 0;                                             // zero array counter

  Serial.print(Number_of_SATS);

  Serial.print(" Satellites Acquired");

  Serial.println(" Start-up Complete");  

}        

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

// This Section Deals with the steering of the rover

void Forward()

{

  Serial.println("Forward");

 

  digitalWrite(leftMotor, HIGH);

  digitalWrite(rightMotor, HIGH);

   

}

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

void Forward_Meter()

{

  digitalWrite(leftMotor, LOW);

  digitalWrite(rightMotor, LOW);

  delay(20);

  digitalWrite(leftMotor, HIGH);

  digitalWrite(rightMotor, HIGH);

  delay(3000);

  digitalWrite(leftMotor, LOW);

  digitalWrite(rightMotor, LOW);

}

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

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

void LeftTurn()

{

  digitalWrite(rightMotor, HIGH);

  digitalWrite(leftMotor, LOW);

  delay(3000);    

  digitalWrite(rightMotor, LOW);

}

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

void RightTurn()

{                                    

  digitalWrite(leftMotor, HIGH);

  digitalWrite(rightMotor, LOW);

  delay(3000);    

  digitalWrite(leftMotor, LOW);                                                                  //delay for 100ms more responsive turn per push on bluetooth  

}

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

void SlowLeftTurn()

{

  digitalWrite(leftMotor,LOW);

  digitalWrite(rightMotor, HIGH);

  // motor2.run(FORWARD);

}

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

// Turning too fast will over-shoot the compass and GPS course

void SlowRightTurn()

{                    

  digitalWrite(rightMotor, LOW);                                                          

  digitalWrite(leftMotor, HIGH);

type or paste code here

}

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

void StopCar()

{
  digitalWrite(leftMotor, LOW);
  digitalWrite(rightMotor, LOW);
}
'''

Dang it the preformatted text forum posting seems to be adding the spacing to the code... =/

Please read the forum instructions about how to post code.

Hopefully the code ill be better now

'''
#include "Wire.h"                                                 // Used by I2C and HMC5883L compass
// #include "I2Cdev.h"                                             // I2C Communications Library (used for compass)
//#include "I2C.h"
#include "HMC5883L.h"                                             // Library for the compass - Download from Github @ https://github.com/jarzebski/Arduino-HMC5883L
                                              // Servo library to control Servo arm for metal detector
#include <SoftwareSerial.h>                                       // Software Serial for Serial Communications - not used
#include <TinyGPS++.h>                                            // Tiny GPS Plus Library - Download from http://arduiniana.org/libraries/tinygpsplus/
                                                                  // TinyGPS++ object uses Hardware Serial 2 and assumes that you have a
                                                                  // 9600-baud serial GPS device hooked up on pins 16(tx) and 17(rx).  
//*******************************************************************************************************
// Digital Motor Output Setup
int leftMotor = 42;
int rightMotor = 43;
                                                                                                                                 

// GPS Variables & Setup//******************************************************************************************************                                                                  


int GPS_Course;                                                    // variable to hold the gps's determined course to destination
int Number_of_SATS;                                                // variable to hold the number of satellites acquired
TinyGPSPlus gps;                                                   // gps = instance of TinyGPS 
                                                                   // pin 17 (blue)   is connected to the TX on the GPS
                                                                   // pin 16 (yellow) is connected to the RX on the GPS

//******************************************************************************************************
                                           // motor speed when moving forward and reverse

//******************************************************************************************************
// Compass Variables & Setup

HMC5883L compass;                                                  // HMC5883L compass(HMC5883L)
int16_t mx, my, mz;                                                // variables to store x,y,z axis from compass (HMC5883L)
int desired_heading;                                               // initialize variable - stores value for the new desired heading
int compass_heading;                                               // initialize variable - stores value calculated from compass readings
int compass_dev = 15;                                               // the amount of deviation that is allowed in the compass heading - Adjust as Needed
                                                                   // setting this variable too low will cause the robot to continuously pivot left and right
                                                                   // setting this variable too high will cause the robot to veer off course

int Heading_A;                                                     // variable to store compass heading
int Heading_B;                                                     // variable to store compass heading in Opposite direction
int pass = 0;                                                      // variable to store which pass the robot is on

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

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

//******************************************************************************************************
// Bluetooth Variables & Setup


int indx;                                                           // command buffer
String str;                                                        // raw string received from android to arduino
int blueToothVal;                                                  // stores the last value sent over via bluetooth
                                                   // Pin 34 of the Aruino Mega used to test the Bluetooth connection status - Not Used

//*****************************************************************************************************
// GPS Locations

unsigned long Distance_To_Home;                                    // variable for storing the distance to destination

int ac =0;                                                         // GPS array counter
int wpCount = 0;                                                   // GPS waypoint counter
double Home_LATarray[6];                                          // variable for storing the destination Latitude - Only Programmed for 5 waypoint
double Home_LONarray[6];                                          // variable for storing the destination Longitude - up to 50 waypoints


int increment = 0;

void setup() 
{  
  Serial.begin(115200);                                            // Serial 0 is for communication with the computer and magnometer.
  // Serial1.begin(11520);                                             // Serial 1 is for Bluetooth communication - DO NOT MODIFY - JY-MCU HC-010 v1.40
  Serial2.begin(9600);                                             // Serial 2 is for GPS communication at 9600 baud - DO NOT MODIFY - Ublox Neo 6m  
  // Motors
  pinMode(leftMotor, OUTPUT);
  pinMode(rightMotor, OUTPUT);
  // bluetooth buffer
  indx = 0;
  // Compass
  while (!compass.begin()) 
  {                       
    Wire.begin();                             // Join I2C bus used for the HMC5883L compass
    compass.begin();                                                 // initialize the compass (HMC5883L)
    compass.setRange(HMC5883L_RANGE_1_3GA);                          // Set measurement range  
    compass.setMeasurementMode(HMC5883L_CONTINOUS);                  // Set measurement mode  
    compass.setDataRate(HMC5883L_DATARATE_15HZ);                     // Set data rate  
    compass.setSamples(HMC5883L_SAMPLES_8);                          // Set number of samples averaged  
    compass.setOffset(65,-290);                                          // Set calibration offset 
  }
  Startup();                                                       // Run the Startup procedure on power-up one time
}

//********************************************************************************************************
// Main Loop

void loop()
{  
  bluetooth();   
  delay(10); // Run the Bluetooth procedure to see if there is any data being sent via BT                                                    
  getGPS();  
  delay(10); // Update the GPS location
  getCompass();
  Serial.println(compass_heading);
  delay(10);
  // Update the Compass Heading
}


// Functionality of Bluetooth
//**************************************************************************************************************************************************
// This procedure reads the serial port - Serial1 - for bluetooth commands being sent from the Android device

void bluetooth()
{
 while (Serial.available()>0)                                    // Read bluetooth commands over Serial1 // Warning: If an error with Serial1 occurs, make sure Arduino Mega 2560 is Selected
 {    
    str = Serial.readStringUntil('0');                      // str is the temporary variable for storing the last sring sent over bluetooth from Android device
    Serial.print(str);                                      // for testing purposes 
    blueToothVal = str.toInt();                               //  convert the string 'str' into an integer and assign it to blueToothVal
    Serial.print("Input Value: ");
    Serial.println(blueToothVal);  
    delay(60);                                                // delay to clear buffer

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

 switch (blueToothVal) 
 {
      case 1:                                
        Serial.println(F("Forward"));
        Forward();
        break;

      case 2:  
        Serial.println(F("Left"));
        LeftTurn();
        StopCar();       
        break;
        
      case 3:                     
        Serial.println(F("Right"));
        RightTurn();
        StopCar();
        break;
        
      case 4:                                            
        Serial.println(F("Stop Car "));
        StopCar();
        break; 

      case 5:                 
        setWaypoint();
        break;
      
      case 6:        
        goWaypoint();
        break;  

      case 7:
        Serial.println(F("GPS Info"));
        gpsInfo();
        break;
      
//      case 15:  
//        pingToggle();
//        break;  

      case 8:
        clearWaypoints();
        break;  

      case 9:                    // finish with waypoints
        ac = 0;
        Serial.print(F("Waypoints Complete"));
        break;

      default:
        Serial.println(F("Invalid Command"));
        break;
 } 
 }                                                              // end of while loop Serial1 read
 
                                                               // if no data from Bluetooth 
   if (Serial.available() < 0)                                 // if an error occurs, confirm that the arduino mega board is selected in the Tools Menu
    {
     Serial.println(F("No Bluetooth Data "));          
    }
  
} // end of bluetooth procedure

// Going to waypoint Code
void goWaypoint()
{   
 Serial.println(F("Go to Waypoint"));
 ac=ac-1;
 while (true)  
  {                                                                // Start of Go_Home procedure 
  // bluetooth();                                                     // Run the Bluetooth procedure to see if there is any data being sent via BT
  // if (blueToothVal == 4)
  //   {
  //   Serial.println("Bluetooth Break");
  //   break;
  //   }                                   // If a 'Stop' Bluetooth command is received then break from the Loop
  if (ac==-1){
    break;
  }
  Serial.println(F("Checking Compass"));
  getCompass();  
  delay(80);// Update Compass heading  
  Serial.println(F("Checking GPS"));                                        
  getGPS(); 
  delay(20);
  // Tiny GPS function that retrieves GPS data - update GPS location// delay time changed from 100 to 10
  
  if (millis() > 5000 && gps.charsProcessed() < 10) 
    {               // If no Data from GPS within 5 seconds then send error
    Serial.println(F("No GPS data: check wiring"));     
    }
  Serial.println(F("Calculating"));
  Distance_To_Home = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location .lng(),Home_LATarray[ac], Home_LONarray[ac]);  //Query Tiny GPS for Distance to Destination
  GPS_Course = TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),Home_LATarray[ac],Home_LONarray[ac]);                               //Query Tiny GPS for Course to Destination   
  Serial.println(F("The Distance to waypoint: "));
  Serial.println(Distance_To_Home);
  Serial.println(F("The GPS Course Needed: "));
  Serial.println(GPS_Course);   
  if (Distance_To_Home <= 1.5)                                   // If the Vehicle has reached it's Destination, then Stop
        {
        StopCar();                                               // Stop the robot after each waypoint is reached
        Serial.println(F("You have arrived!"));                    // Print to Bluetooth device - "You have arrived"          
        ac--;                                                    // increment counter for next waypoint
        if (ac==-1) {
          break;
          }                                                   // Break from Go_Home procedure and send control back to the Void Loop                                                                  // go to next waypoin      
        }   
   else if ( abs(GPS_Course - compass_heading) <= 30)                  // If GPS Course and the Compass Heading are within x degrees of each other then go Forward                                                                                                                                    
       {                                                          // otherwise find the shortest turn radius and turn left or right
         Forward();                                               // Go Forward
       } else 
         {                                                       
            int x = (GPS_Course - 360);                           // x = the GPS desired heading - 360
            int y = (compass_heading - (x));                      // y = the Compass heading - x
            int z = (y - 360);                                    // z = y - 360
            
            if ((z <= 180) && (z >= 0))                           // if z is less than 180 and not a negative value then turn left otherwise turn right
                  { 
                    Serial.println(F("Turning Left:"));
                    SlowLeftTurn();
                      }
             else 
             { 
               Serial.println(F("Turning Right"));
               SlowRightTurn(); 
             }               
          } 
  }                                                              // End of While Loop 
}                                                                // End of Go_Home procedure

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

void getGPS()                                                 // Get Latest GPS coordinates
{
    while (Serial2.available() > 0)
    gps.encode(Serial2.read());
    delay(60);
} 

// *************************************************************************************************************************************************
 
void setWaypoint()                                            // Set up to 5 GPS waypoints
{

//if ((wpCount >= 0) && (wpCount < 50))
if (wpCount >= 0)
  {
    Serial.print("GPS Waypoint ");
    Serial.print(wpCount + 1);
    Serial.print(" Set ");
    getGPS();                                                 // get the latest GPS coordinates
    getCompass();                                             // update latest compass heading                               
    Home_LATarray[ac] = gps.location.lat();                   // store waypoint in an array   
    Home_LONarray[ac] = gps.location.lng();                   // store waypoint in an array                                                              
    Serial.print("Waypoint #1: ");
    Serial.print(Home_LATarray[0],6);
    Serial.print(" ");
    Serial.println(Home_LONarray[0],6);
    Serial.print("Waypoint #2: ");
    Serial.print(Home_LATarray[1],6);
    Serial.print(" ");
    Serial.println(Home_LONarray[1],6);
    Serial.print("Waypoint #3: ");
    Serial.print(Home_LATarray[2],6);
    Serial.print(" ");
    Serial.println(Home_LONarray[2],6);
    Serial.print("Waypoint #4: ");
    Serial.print(Home_LATarray[3],6);
    Serial.print(" ");
    Serial.println(Home_LONarray[3],6);
    Serial.print("Waypoint #5: ");
    Serial.print(Home_LATarray[4],6);
    Serial.print(" ");
    Serial.println(Home_LONarray[4],6);
    wpCount++;                                                  // increment waypoint counter
    ac++;                                                       // increment array counter       
  }         
  else {Serial.print("Waypoints Full");}
}

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

void clearWaypoints()
{
   memset(Home_LATarray, 0, sizeof(Home_LATarray));             // clear the array
   memset(Home_LONarray, 0, sizeof(Home_LONarray));             // clear the array
   wpCount = 0;                                                 // reset increment counter to 0
   ac = 0;  
   Serial.println("GPS Waypoints Cleared");                      // display waypoints cleared  
}

 // *************************************************************************************************************************************************
 
void getCompass()                                               // get latest compass value
 {  
  Serial.println(F("In getCompass"));
  Vector norm = compass.readNormalize();

  float declinationAngle = 0.418;

  // Calculate heading
  Serial.println(F("Before Atan"));
  float heading = atan2(norm.YAxis, norm.XAxis);
  heading += declinationAngle;
  // heading = heading-0.4;
  Serial.println(F("Positive Balance"));
  if(heading < 0)
     heading += 2 * M_PI;  
  Serial.println(F("Radians to degrees"));    
  compass_heading = (int)(heading * 180/M_PI);  
  delay(100);                 // assign compass calculation to variable (compass_heading) and convert to integer to remove decimal places                                                              
  Serial.println(F("Get Compass Finished"));
 }
 
void gpsInfo()                                                  // displays Satellite data to user
  {
        Number_of_SATS = (int)(gps.satellites.value());         //Query Tiny GPS for the number of Satellites Acquired 
        Distance_To_Home = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),Home_LATarray[ac], Home_LONarray[ac]);  //Query Tiny GPS for Distance to Destination    
        Serial.print("Lat:");
        Serial.print(gps.location.lat(),6);
        Serial.print(" Lon:");
        Serial.print(gps.location.lng(),6);
        Serial.print(" ");
        Serial.print(Number_of_SATS); 
        Serial.print(" SATs ");
        
        Serial.print(Distance_To_Home);
        Serial.print("m"); 
Serial.print("Distance to Home ");
Serial.println(Distance_To_Home);
  
  }
  // Start-up function to be run inside of set-up and to initialise the rover for use

void Startup()
{
     
             
     for (int i=5; i >= 1; i--)                       // Count down for X seconds
      {         
        Serial.print("Pause for Startup... "); 
        Serial.print(i);
        delay(1000);                                   // Delay for X seconds
      }    
    
  
  Serial.println("Searching for Satellites "); 
      
  while (Number_of_SATS < 0)                         // Wait until x number of satellites are acquired before starting main loop
  {              
    Serial.println("Small delay for home co-ordinates to be recieved");
    delay(3000);                    
    getGPS();                                         // Update gps data
    Serial.println(int(gps.satellites.value()));
    Number_of_SATS = (int)(gps.satellites.value());   // Query Tiny GPS for the number of Satellites Acquired       
    bluetooth();                                      // Check to see if there are any bluetooth commands being received  
    Serial.println("Looping for SATS"); 
    gpsInfo();
  }                                        // set intial waypoint to current location
  wpCount = 0;                                        // zero waypoint counter
  ac = 0;                                             // zero array counter
    
  Serial.print(Number_of_SATS);
  Serial.print(" Satellites Acquired");
  Serial.println(" Start-up Complete");   
}        

// This Section Deals with the steering of the rover
void Forward()
{
//  Ping();
  Serial.println("Forward");
  // motor1.setSpeed(mtr_Spd);                                                   
  // motor2.setSpeed(mtr_Spd);     
  // motor1.run(FORWARD);                                                         // go forward all wheels 
  // motor2.run(FORWARD);
  digitalWrite(leftMotor, HIGH);
  digitalWrite(rightMotor, HIGH);
  // delay(1000);
  // digitalWrite(leftMotor, LOW);
  // digitalWrite(rightMotor, LOW);
  
}

void Forward_Meter()
{
  // motor1.setSpeed(mtr_Spd);                                                   
  // motor2.setSpeed(mtr_Spd);     
  // motor1.run(FORWARD);                                                        // go forward all wheels for specified time
  // motor2.run(FORWARD);
  digitalWrite(leftMotor, LOW);
  digitalWrite(rightMotor, LOW);
  delay(20);
  digitalWrite(leftMotor, HIGH);
  digitalWrite(rightMotor, HIGH);
  delay(3000);
  digitalWrite(leftMotor, LOW);
  digitalWrite(rightMotor, LOW);
}

void LeftTurn()
{
  digitalWrite(rightMotor, HIGH);
  digitalWrite(leftMotor, LOW);
  delay(3000);    
  digitalWrite(rightMotor, LOW);
}

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

void RightTurn()
{                                           
  digitalWrite(leftMotor, HIGH);
  digitalWrite(rightMotor, LOW);
  delay(3000);    
  digitalWrite(leftMotor, LOW);                                                                  //delay for 100ms more responsive turn per push on bluetooth  
}

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


void SlowLeftTurn()
{
  digitalWrite(leftMotor,LOW);
  digitalWrite(rightMotor, HIGH);
}


void SlowRightTurn()
{                     
  digitalWrite(rightMotor, LOW);                                           
  digitalWrite(leftMotor, HIGH);   
}

void StopCar()
{
  digitalWrite(leftMotor, LOW);
  digitalWrite(rightMotor, LOW); 
}

updated getCompass()

void getCompass()                                               // get latest compass value
 {  
  Serial.println(F("In getCompass"));
  Vector norm = compass.readNormalize();
  Serial.println(F("Vector Done Float Next"));
  // float declinationAngle = 0.418;
  // Calculate heading
  Serial.println(F("Before Atan"));
  float heading = atan2(norm.YAxis, norm.XAxis);
  Serial.println(F("Values in Atan"));
  Serial.println(norm.YAxis);
  Serial.println(norm.yAxis);
  // heading += declinationAngle;
  // heading = heading-0.4;
  Serial.println(F("Positive Balance"));
  if(heading < 0)
     heading += 2 * M_PI;  
  Serial.println(F("Radians to degrees"));    
  compass_heading = (int)(heading * 180/M_PI);  
  delay(100);                 // assign compass calculation to variable (compass_heading) and convert to integer to remove decimal places                                                              
  Serial.println(F("Get Compass Finished"));
 }

Okay so I must admit I now really have no idea what is going on. On using the updated which commented out the following two lines

// float declinationAngle = 0.418;
// heading += declinationAngle;

the code appears to be working (not connected to the actual wheels at the moment so was standing still) with the output

In getCompass
Vector Done Float Next
Before Atan
Values in Atan
-80.04
-29.44
Positive Balance
Radians to degrees
Get Compass Finished
249
6
Input Value: 6
Go to Waypoint
Checking Compass
In getCompass
Vector Done Float Next
Before Atan
Values in Atan
-138.92
-66.24
Positive Balance
Radians to degrees
Get Compass Finished
Checking GPS
Calculating
The Distance to waypoint: 
15602787
The GPS Course Needed: 
185
Turning Left:
Checking Compass
In getCompass
Vector Done Float Next
Before Atan
Values in Atan
-115.92
-61.64
Positive Balance
Radians to degrees
Get Compass Finished
Checking GPS
Calculating
The Distance to waypoint: 
15602787
The GPS Course Needed: 
185
Turning Left:

The above ouput was cut from the total output as I left it running for 5 minutes (producing thousands of lines of output)
My question now is why is the float causing the freeze and what would be the best way of normalising the magnetic angle to avoid it.

What memory usage does the compiler report when you compile?

try making it 'const'

so i placed the const float declinationAngle out of the function with the rest of the variable declarations and at this stage it appears to have solved the issue although I can't field test it at the moment due to poor weather (storms and the need to run it outside for the gps with the vehicle not yet being weather proofed). Memory usage is at 27%.

Update: After a further 24hrs when testing the vehicle again the error has returned. Now it stops at the line Vector norm = compass.normalize(); I tried making all the variables in the getCompass(); global rather than local, but this had no effect on the program execution. I am at a complete loss.