calculate relative direction to destination

here is the code that i am using for my car to easily navigate through waypoints but its not doing so its showing me right only why can any body help me here please

#include <math.h>
#include <nmea.h>
#include <Wire.h>
#include <LiquidCrystal.h>
LiquidCrystal myLCD(42, 40, 38, 36, 34, 32);
float lat; 
float lon;
NMEA myGPS(GPRMC);
float dest_latitude = 25.37104;
float dest_longitude = 68.35567;

int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41;
const int potPin = A8;
const int leftMotor = 27; //PWM pin to the L293
const int rightMotor = 26; //PWM pin to the L293
int     motorA1=22;
int     motorA2=23;

char Dir;
bool isLeft()          {
  return (analogRead(potPin)/4) <65;
}
bool isLeftOfCenter()  {
  return (analogRead(potPin)/4) < 108;
}
bool isRightOfCenter() {
  return (analogRead(potPin)/4) > 118;
}
bool isRight()         {
  return (analogRead(potPin)/4) > 128;
}

int beeperpin=44;
void setup() {
  Serial1.begin(4800);
  //Serial.begin(9600);
  pinMode(leftMotor,OUTPUT);
  pinMode(rightMotor,OUTPUT);
  pinMode(motorA1,OUTPUT);
  pinMode(motorA2,OUTPUT);
  //Serial.print("GPS BASED`");
  myLCD.begin(16,2);
  myLCD.print("GPS BASED");
  delay(3000);
  myLCD.clear();
  myLCD.home();

  Wire.begin();
  HMC6352SlaveAddress = HMC6352SlaveAddress >> 1;
  //pinMode(beeperpin,OUTPUT);
  //pinMode(13,OUTPUT);

}
void loop(){
  if (Serial1.available() > 0 ) {
    // read incoming character from GPS
   if (myGPS.decode(Serial1.read())) {

    // check if the character completes a valid GPS sentence
    
      // check if GPS positioning was active
      if (myGPS.gprmc_status() == 'A') {

 
    
      //         float lat; 
      //        float lon;
      lat=myGPS.gprmc_latitude();
      lon = myGPS.gprmc_longitude();
      Serial.print("D:");
      int d=(round(myGPS.gprmc_distance_to(dest_latitude, dest_longitude, MTR)));
      // Serial.print(d);
      // Serial.print("m");
      myLCD.setCursor(0, 0);
      myLCD.print(d);
      myLCD.print('m');
      if (d < 6){
        myLCD.clear();
        myLCD.home();
        myLCD.setCursor(0,0);
        myLCD.print("Destinat");
        digitalWrite(13, HIGH);
        digitalWrite(beeperpin, HIGH);
        carbreak();
      }
     else if (d > 6){
        myLCD.clear();
        myLCD.home();
        myLCD.setCursor(0, 0);
        myLCD.print(d);
        myLCD.print('m');// digitalWrite(beeperpin, LOW);
        //digitalWrite(13, LOW);
      
      float mydir= 0;
      for (int x = 0; x<20; x=x+1){
        mydir=mydir + calc_bearing (lat, lon, dest_latitude, dest_longitude);
      }

      mydir=mydir/20;

      if (mydir < 0) { 
        mydir += 360; 
      }
      if (mydir > 180) { 
        mydir -= 360; 
      }

      float currentdir = compassvalue();
      float finaldir =currentdir - mydir;
      myLCD.setCursor(3,1);
      if ( abs (finaldir) < 20)
      {
        // Serial.print ('=');
        myLCD.print ("=");
        steering('C');
        delay(500);
        motorForward();
      }
      else{


        if (finaldir < 0)
        {
          //  Serial.print ('<');
          myLCD.print ("L"); 
          steering('L');
          delay(300);
          motorForward();
          delay(300);
          carbreak();
          delay(500);
        }
        if (finaldir > 0)
        {
          //   Serial.print ('>');
          myLCD.print ("R");
          steering('R');
          delay(300);
          motorForward();
          delay(300);
          carbreak();  
          delay(500);
        }
        // Serial.print(abs(finaldir), DEC);
        myLCD.print(abs(finaldir), DEC);
        // myLCD.print(223, BYTE);  // print °-character
      }
     }

    }
  }
}
}

int calc_bearing(float flat1, float flon1, float flat2, float flon2)
{
  float calc;
  float bear_calc;

  float x = 69.1 * (flat2 - flat1); 
  float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);

  calc=atan2(y,x);

  bear_calc= degrees(calc);

  if(bear_calc<=1){
    bear_calc=360+bear_calc; 
  }
  return bear_calc;
}

int compassvalue(){
  //"Get Data. Compensate and Calculate New Heading"
  Wire.beginTransmission(HMC6352SlaveAddress);
  Wire.send(HMC6352ReadAddress);              // The "Get Data" command
  Wire.endTransmission();

  //time delays required by HMC6352 upon receipt of the command
  //Get Data. Compensate and Calculate New Heading : 6ms
  delay(6);

  Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB

  //"The heading output data will be the value in tenths of degrees
  //from zero to 3599 and provided in binary format over the two bytes."
  byte MSB = Wire.receive();
  byte LSB = Wire.receive();

  float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
  float headingInt = headingSum / 10; 

  //Serial.print(headingInt);
  //Serial.println(" degrees");

  return headingInt;
}

void motorStop() {
  digitalWrite(leftMotor, LOW); 
  digitalWrite(rightMotor, LOW);
}
void motorLeft() {
  digitalWrite(leftMotor, HIGH);
}
void motorRight() {
  digitalWrite(rightMotor, HIGH);
}

//back wheels
void motorForward()
{
  digitalWrite(motorA1,HIGH);
  digitalWrite(motorA2,LOW);
}

void motorBackward()
{
  digitalWrite(motorA1,LOW);
  digitalWrite(motorA2,HIGH);
}


void carbreak()
{

  digitalWrite(motorA1,LOW);
  digitalWrite(motorA2,LOW);
}
void steering(char Dir)
{
  //if (Serial.available() == 0)
  //return;
  //Dir=toupper(Serial.read());
  switch (Dir)
  {
  case 'L':
    //Serial.println("New command: L");
    if (isLeft())
    {
    }//Serial.println("Already Left");
    else
    {
      //Serial.println("Heading Left");
      motorLeft();
      while (!isLeft()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Left");
    }
    break;

  case 'C':
    //Serial.println("New command: C");
    if (isRightOfCenter())
    {
      //Serial.println("Heading Left toward Center");
      motorLeft();
      while (isRightOfCenter()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Centered");
    }
    else
      if (isLeftOfCenter())
      {
        //Serial.println("Heading Right toward Center");
        motorRight();
        while (isLeftOfCenter()) /* JUST WAITING */;
        motorStop();
        //Serial.println("Is now Centered");
      }
      else
      {
      }
    //Serial.println("Already Centered");
    break;
  case 'R':
    //Serial.println("New command: R");
    if (isRight())
    {
    }//Serial.println("Already Right");
    else
    {
      //Serial.println("Heading Right");
      motorRight();
      while (!isRight()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Right");
    }
    break;
  }  // end of switch on direction
}

Again a bunch of code and no debug output.

well my board is attached with the LCDs and the motor i have debug all the GPS and Compass seperately and combine as well i think its the coding that is not Properly defining the robot in which direction to go

I think so too, but since youhaven't presented any evidence, we're both just guessing. I don't want to play your games again.

i am just worried with the calculation that i have made in the code the code is doing good but its the calculation that is creating problem

Well, you seem to have the usual potential mistake of testing for greater than or less than, but not equal, but without seeing your debug output, we’ll be playing 20 x 103 questions again .

here is the code i have commented the whole code as i am unable to show the Debug output

#include <math.h>
#include <nmea.h>
#include <Wire.h>
#include <LiquidCrystal.h>
LiquidCrystal myLCD(42, 40, 38, 36, 34, 32);
float lat; 
float lon;
NMEA myGPS(GPRMC);
float dest_latitude = 25.37104;//the destination Latitude 
float dest_longitude = 68.35567;//the destination Longitude

int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41;
const int potPin = A8;          //Analog pin A8 for reading Potientiometer that drives the Servo steering
const int leftMotor = 27; //Pin connected to the Relay pin for moving the steering to left
const int rightMotor = 26;//Pin connected to the Relay pin for moving the steering to right
int     motorA1=22;       //Pin connected to the Relay pin for moving the car forward
int     motorA2=23;        //Pin connected to the Relay pin for moving the car backward
//below code describes the Steering ranges for left right and center
char Dir;
bool isLeft()          {
  return (analogRead(potPin)/4) <65;
}
bool isLeftOfCenter()  {
  return (analogRead(potPin)/4) < 108;
}
bool isRightOfCenter() {
  return (analogRead(potPin)/4) > 118;
}
bool isRight()         {
  return (analogRead(potPin)/4) > 128;
}

int beeperpin=44;//the following pin is connected to the buzzer pin 
void setup() {
  Serial1.begin(4800);//Start the serial communication of GPS from the Mega Board
  //Serial.begin(9600);


  //PIN DESCRIPTION
  pinMode(leftMotor,OUTPUT);
  pinMode(rightMotor,OUTPUT);
  pinMode(motorA1,OUTPUT);
  pinMode(motorA2,OUTPUT);
  //Serial.print("GPS BASED`");
  myLCD.begin(16,2);//starting the LCD
  myLCD.print("GPS BASED");//Printing on LCD
  delay(3000);
  myLCD.clear();//Clearing the LCD
  myLCD.home();

  Wire.begin();
  HMC6352SlaveAddress = HMC6352SlaveAddress >> 1;
  //pinMode(beeperpin,OUTPUT);
  //pinMode(13,OUTPUT);

}
void loop(){
  if (Serial1.available() > 0 ) {
    // read incoming character from GPS
    if (myGPS.decode(Serial1.read())) {

      // check if the character completes a valid GPS sentence

      // check if GPS positioning was active
      if (myGPS.gprmc_status() == 'A') {
        //         float lat; 
        //        float lon;
        lat=myGPS.gprmc_latitude();//Current latitude
        lon = myGPS.gprmc_longitude();//Current longitude
        //Serial.print("D:");
        int d=(round(myGPS.gprmc_distance_to(dest_latitude, dest_longitude, MTR)));
        //calculate the distance by rounding the GPS data from the current latitude,longitude to the desired Longitude 
        // Serial.print(d);
        // Serial.print("m");
        myLCD.setCursor(0, 0);
        myLCD.print(d);//Print the Distnace on LCD
        myLCD.print('m');
        if (d < 6){//if the distance is less than 6 than it should 
          myLCD.clear();
          myLCD.home();
          myLCD.setCursor(0,0);
          myLCD.print("Destinat");//Print destination 
          digitalWrite(13, HIGH);//Put the LED high
          digitalWrite(beeperpin, HIGH);//On the Buzzer pin 
          carbreak();//apply car breaks 
        }
        else if (d > 6){//if the distnace is not greater than 6 then do the following 
          myLCD.clear();
          myLCD.home();
          myLCD.setCursor(0, 0);
          myLCD.print(d);
          myLCD.print('m');// digitalWrite(beeperpin, LOW);
          //digitalWrite(13, LOW);
          //average the down the direction from current latitude,longitude to destination latitude,longitude
          float mydir= 0;
          for (int x = 0; x<20; x=x+1){
            mydir=mydir + calc_bearing (lat, lon, dest_latitude, dest_longitude);
          }
          mydir=mydir/20;
          if (mydir < 0) {//thats what i not understand 
            mydir += 360; //thats what i not understand
          }
          if (mydir > 180) { //thats what i not understand
            mydir -= 360; //thats what i not understand
          }

          float currentdir = compassvalue();//its the compass value
          float finaldir =currentdir - mydir;//subtract the compass value form the GPS beairng
          myLCD.setCursor(3,1);
          if ( abs (finaldir) < 20)//if the final direction is +20 or -20 it should go straight
          {
            // Serial.print ('=');
            myLCD.print ("=");
            steering('C');//Steering Centered
            delay(500);
            motorForward();//car Forward
          }
          else{


            if (finaldir < 0)//if the Final direction is less than zero than goto LEFT
            {
              //  Serial.print ('<');
              myLCD.print ("L"); 
              steering('L');
              delay(300);
              motorForward();
              delay(300);
              carbreak();
              delay(500);
            }
            if (finaldir > 0)//if the Final direction is greater than zero than goto RIGHT
            {
              //   Serial.print ('>');
              myLCD.print ("R");
              steering('R');
              delay(300);
              motorForward();
              delay(300);
              carbreak();  
              delay(500);
            }
            // Serial.print(abs(finaldir), DEC);
            myLCD.print(abs(finaldir), DEC);//print the Direction 
            // myLCD.print(223, BYTE);  // print °-character
          }
        }

      }
    }
  }
}

int calc_bearing(float flat1, float flon1, float flat2, float flon2)
{
  float calc;
  float bear_calc;

  float x = 69.1 * (flat2 - flat1); //thats what i not understand
  float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);//thats what i not understand

  calc=atan2(y,x);

  bear_calc= degrees(calc);

  if(bear_calc<=1){//thats what i not understand
    bear_calc=360+bear_calc; //thats what i not understand
  }
  return bear_calc;//thats what i not understand
}

int compassvalue(){
  //"Get Data. Compensate and Calculate New Heading"
  Wire.beginTransmission(HMC6352SlaveAddress);
  Wire.send(HMC6352ReadAddress);              // The "Get Data" command
  Wire.endTransmission();

  //time delays required by HMC6352 upon receipt of the command
  //Get Data. Compensate and Calculate New Heading : 6ms
  delay(6);

  Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB

  //"The heading output data will be the value in tenths of degrees
  //from zero to 3599 and provided in binary format over the two bytes."
  byte MSB = Wire.receive();
  byte LSB = Wire.receive();

  float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
  float headingInt = headingSum / 10; 

  //Serial.print(headingInt);
  //Serial.println(" degrees");

  return headingInt;
}
//Motor commands Description 
void motorStop() {
  digitalWrite(leftMotor, LOW); 
  digitalWrite(rightMotor, LOW);
}
void motorLeft() {
  digitalWrite(leftMotor, HIGH);
}
void motorRight() {
  digitalWrite(rightMotor, HIGH);
}

//back wheels
void motorForward()
{
  digitalWrite(motorA1,HIGH);
  digitalWrite(motorA2,LOW);
}

void motorBackward()
{
  digitalWrite(motorA1,LOW);
  digitalWrite(motorA2,HIGH);
}


void carbreak()
{

  digitalWrite(motorA1,LOW);
  digitalWrite(motorA2,LOW);
}
void steering(char Dir)
{
  //if (Serial.available() == 0)
  //return;
  //Dir=toupper(Serial.read());
  switch (Dir)
  {
  case 'L':
    //Serial.println("New command: L");
    if (isLeft())
    {
    }//Serial.println("Already Left");
    else
    {
      //Serial.println("Heading Left");
      motorLeft();
      while (!isLeft()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Left");
    }
    break;

  case 'C':
    //Serial.println("New command: C");
    if (isRightOfCenter())
    {
      //Serial.println("Heading Left toward Center");
      motorLeft();
      while (isRightOfCenter()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Centered");
    }
    else
      if (isLeftOfCenter())
      {
        //Serial.println("Heading Right toward Center");
        motorRight();
        while (isLeftOfCenter()) /* JUST WAITING */;
        motorStop();
        //Serial.println("Is now Centered");
      }
      else
      {
      }
    //Serial.println("Already Centered");
    break;
  case 'R':
    //Serial.println("New command: R");
    if (isRight())
    {
    }//Serial.println("Already Right");
    else
    {
      //Serial.println("Heading Right");
      motorRight();
      while (!isRight()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Right");
    }
    break;
  }  // end of switch on direction
}

if (d > 6){//if the distnace is not greater than 6 then do the following

The comments do not reflect the code, and the statement "do the following" is not informative either. THis does not help ...

No time to dive in the code to see what's happening (too nice weather :).

cutebuddy6: well my board is attached with the LCDs and the motor i have debug all the GPS and Compass seperately and combine as well

How about debugging to the LCD ? It seems to be your best option for working out which part of the system is failing.

Well do the following means that follow the code that is with in the condition

i have checked the LCD as well its working fine

I see words in that last post, but not much meaning or information.

i have commented the whole code as i am unable to show the Debug output

Static analysis is just about the hardest debug technique known.

Good luck.

cutebuddy6: i have checked the LCD as well its working fine

That is good news. What I meant was you could output the values of key variables to the LCD so you can see what's going on inside your sketch as it runs.

I have a project on my desk right now that uses the serial pins so cannot run the serial monitor, so similar situation to yours. My LCD is full of messages that won't be there when I go live, but help understand if the sketch is doing as I expect it to. It means in some cases adding delays so you can read them, but I can't see an easier way for you to work out what's going wrong than to use the display and watch what it tells you.

can any body guide me with the calculation of my GPS

My guess is that “compassvalue()” and mydir use different ranges of angles and currentdir - mydir is coming out positive and therefore always steering right. Display ‘finaldir’ on the LCD. If it ever shows more than 180 or less than -180 you have a mistake.

if (mydir < 0) {//thats what i not understand
mydir += 360; //thats what i not understand
}
if (mydir > 180) { //thats what i not understand
mydir -= 360; //thats what i not understand
}

John just beat me :slight_smile:

the if (mydir < 0) is indeed strange as the calc_bearing returns a angle > 1, ==> why do you test it?

For the rest, JOhn is right, this piece of code takes care that you always have a direction between -180 and 180. That makes it easier to detect left and right.

Think you should minimize the program as much as possible to understand it part by part, it is rather big and the program flow is not very clear. There should be a level of abstraction at the top that gives oversight. I am missing that level, it should write the outline in less than 10 function calls / statements. IN pseudo code it often boils down to 4 steps

loop()
{
// READ SENSOR
readGps();

// DO THE MATH
calcNewDistance();
calcNewDirection();

// INFORM THE USER
informUser(); // lcd.print(all you wanna print);

// DO THE ACTIONS
repositionCar();
Go();
}

Advice
– make a new project and start stripping until you only do GPS readings.
– make a project that only does the compass reading.

Do you have a link to the NMEA library used? Think that is a crucial

thanks for your advice.

John you are right its shows me continuously and some times a left when i continuously rotate my car

Advice -- make a new project and start stripping until you only do GPS readings. -- make a project that only does the compass reading.

Do you have a link to the NMEA library used? Think that is a crucial

yes i have the Link to the NMEA library

i have the GPS reading and the compass reading separately

Do you have a link to the NMEA library used? Think that is a crucial

yes i have the Link to the NMEA library

Well, that's nice. I think maybe the idea was that you posted the link.

As usual, you appear to be ignoring the advice being given. Send debugging information to the LCD. Put a delay in to give you time to read it, and make notes. That way you can figure out what is going on, or if you can't, you can present something that might allow others to figure it out.

If you have a laptop or netbook, sit it in the car, and use it to collect debugging data from Serial.

here is the Link for the Library i used http://www.maartenlamers.com/nmea/

second can you tell me the piece of code where should i edit the code and show it on the LCD

second can you tell me the piece of code where should i edit the code and show it on the LCD

Anywhere you have a "Serial" debug print, and probably a few more where you currently don't.

is there a particular reason you didn't use the "course_to" method?