The code can't run continual, can only print few words, why?

Hi,
the code attached can only serial monitor few words, why?
Thanks.
Adam

#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <HMC5883L.h>
#include <math.h>
/*
   This sample sketch demonstrates the normal use of a TinyGPS++ (TinyGPSPlus) object.
   It requires the use of SoftwareSerial, and assumes that you have a
   4800-baud serial GPS device hooked up on pins 4(rx) and 3(tx).
*/
static const int RXPin = 2, TXPin = 3;
static const uint32_t GPSBaud = 9600;
double compass_headingDegrees;
int L1 = 4;
int L2 = 5;
int L3 = 6;
int L4 = 7;


// The TinyGPS++ object
TinyGPSPlus gps;
// the HMC5883L object
HMC5883L compass;

// The serial connection to the GPS device
SoftwareSerial ss(RXPin, TXPin);

void setup()
{
  Serial.begin(115200);

  Serial.print("File   : "), Serial.println(__FILE__);
  Serial.print("Date   : "), Serial.println(__DATE__);

  Serial.print("setup");
  ss.begin(GPSBaud);
  pinMode(13, OUTPUT);
  pinMode(L1, OUTPUT);
  pinMode(L2, OUTPUT);
  pinMode(L3, OUTPUT);
  pinMode(L4, OUTPUT);
  // Set measurement range
  compass.setRange(HMC5883L_RANGE_1_3GA);

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

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

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

  // Set calibration offset. See HMC5883L_calibration.ino
  compass.setOffset(0, 0);
}

void loop()
{
  // This sketch displays information every time a new sentence is correctly encoded.
  while (ss.available() > 0)
    if (gps.encode(ss.read()))
      gps_track();
  if (millis() > 5000 && gps.charsProcessed() < 10)
  {
    Serial.println(F("No GPS detected: check wiring."));
    while (true);
  }

}

void gps_track()
{
  //*******************compasss data ****************************//
  Vector norm = compass.readNormalize();

  // Calculate heading
  double heading = atan2(norm.YAxis, norm.XAxis);

  // Set declination angle on your location and fix heading
  // You can find your declination on: http://magnetic-declination.com/
  // (+) Positive or (-) for negative
  // For Bytom / Poland declination angle is 4'26E (positive)
  // Formula: (deg + (min / 60.0)) / (180 / M_PI);
  double declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / M_PI);
  heading += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0)
  {
    heading += 2 * PI;
  }

  if (heading > 2 * PI)
  {
    heading -= 2 * PI;
  }

  // Convert to degrees
  compass_headingDegrees = heading * 180 / M_PI;
  if (gps.location.isValid())
  {
    double gps_headingDeg;
    double distance_cal;
    double headingRad;
    double long1 = gps.location.lng() ;
    double lat1 = gps.location.lat();
    double long2 = 87.2345  ;
    double lat2 = 22.1167   ;
    double diflong;
    double diflat;
    double a;
    double c;
    double error;
    lat1 = radians(lat1);
    long1 = radians(long1);
    lat2 = radians(lat2);
    long2 = radians(long2);
    diflong = long1 - long2;
    diflat = lat1 - lat2;
    headingRad = atan2((sin(diflong) * cos(lat2)), ((cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(diflong))));
    gps_headingDeg = (180 * headingRad) / 3.14159;

    if (gps_headingDeg > 0) {
      gps_headingDeg = gps_headingDeg;
    } else {
      gps_headingDeg = gps_headingDeg + 360;
    }

    error = (gps_headingDeg - compass_headingDegrees);
    if (error < -180) {
      error = error + 360;

    }
    if (error > 180) {
      error = error - 360;


    }

    //*******distance calculation
    a = (sin(diflat / 2) * sin(diflat / 2)) + (cos(lat1) * cos(lat2) * (sin(diflong / 2) * sin(diflong / 2)));
    c = 2 * atan2(sqrt(a), sqrt(1 - a));
    distance_cal = c * 6371000.0; // gives in meters
    //////////****************************error calculation
    Serial.print("gps heading");
    Serial.println(gps_headingDeg);
    Serial.print("magnetic heading");
    Serial.println(compass_headingDegrees);
    Serial.print("error");
    Serial.println(error);
    Serial.print("distance");
    Serial.println(distance_cal);
    bool condition = ((compass_headingDegrees - 10) < gps_headingDeg) && ((compass_headingDegrees + 10) > gps_headingDeg);
    if (condition) {

      fwr();
    } else {
      if (error > 10) {
        rgt();
      }
      if (error < -10) {
        left();
      }
    }
  }
}
void fwr()
{
  digitalWrite(L1, HIGH);
  digitalWrite(L2, LOW);
  digitalWrite(L3, HIGH);
  digitalWrite(L4, LOW);
}
void left() {
  digitalWrite(L1, LOW);
  digitalWrite(L2, HIGH);
  digitalWrite(L3, HIGH);
  digitalWrite(L4, LOW);
}
void rgt() {
  digitalWrite(L1, HIGH);
  digitalWrite(L2, LOW);
  digitalWrite(L3, LOW);
  digitalWrite(L4, HIGH);
}

sm a_gps_car

Try putting a slight delay or a while loop waiting for the serial to had done its startup thing.

like this

Serial.begin(400035678834.7);
while (!Serial) 
 { 
   ; // Wait for serial to connect 
 }

try commenting out code until it starts to run in order to isolate that cause of the problem

Put Serial.flush() after each print to wait for the text to be sent.

Does the compass need a .begin or other initialization?

2 Likes

Thank you!