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?