Moving UGV(unmanned ground vehicle) based on two waypoints

Hello there, higher level genius people,

I am having problem here with the coding.
My Objective is to move UGV auto based on two waypoints by using compass HMC5883L
and GPS SKM53. Other than that im using motor sheild L298P.

First i am try move both motor using shield L298P,

here’s the code

// This motor shield use Pin 4,5,6,7, to control the motor
// Through serial monitor, type 'w','s','a','d','o' to control the motor directions

int E1 = 6; 
int M1 = 7;     
int E2 = 5; 
int M2 = 4;     

void Motor1(int pwm, boolean reverse)
        { 
            analogWrite(E1,pwm); //set pwm control, 0 for stop, and 255 for maximum speed
            if(reverse)
          { 
            digitalWrite(M1,HIGH);
           } 
            else
           { 
            digitalWrite(M1,LOW);
            } 
            } 
         
void Motor2(int pwm, boolean reverse) 
        { 
           analogWrite(E2,pwm); 
           if(reverse) 
         { 
           digitalWrite(M2,HIGH);
           } 
            else 
           { 
            digitalWrite(M2,LOW); 
            }
            } 
           
void setup() 
        { 
            int i; 
            for(i=4;i<=7;i++)         //For Arduino Motor Shield,set pin 4,5,6,7 to output mode
            pinMode(i, OUTPUT);       
    
            Serial.begin(9600); 
         }
   
            void loop() 
         { 
            int x,delay_en; 
            char val; 
            while(1) 
         { 
            val = Serial.read(); 
            if(val!=-1) 
               { 
                 switch(val) 
               {
                 
                 case 'w': //move ahead 
                           Motor1(150,true); 
                           Motor2(150,true); 
                           break;
                 
                 case 's': //Move back 
                           Motor1(150,false); 
                           Motor2(150,false); 
                           break;   
                 
                 case 'a': //turn left 
                           Motor1(150,false); 
                           Motor2(150,true); 
                           break;
                 
                 case 'd': //turn right 
                           Motor1(150,true); 
                           Motor2(150,false); 
                           break;
                           
                 case 'o': //stop 
                           Motor1(0,false); 
                           Motor2(0,false); 
                           break;
                          } 
                   }  
           }  
   }

its work perfectly,

2nd step i setup my compass HMC5883L alone to get the degree (when rotate)
here the code :

#include <Wire.h>
#include <HMC5883L.h>

HMC5883L compass;
int error = 0;

void setup() 
{
  Serial.begin(9600);
  Serial.println("Serial started.");
  Wire.begin();
  
  Serial.println("Constructing new HMC5883L");
  compass = HMC5883L();

  Serial.println("Setting scale to +/- 1.3 Ga");
  int error = compass.SetScale(1.3); // Set the scale of the compass.
  if(error != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));

  Serial.println("Setting measurement mode to continuous.");
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  if(error != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));
}

void loop() 
{
  MagnetometerRaw raw = compass.ReadRawAxis();
  MagnetometerScaled scaled = compass.ReadScaledAxis();
  
  int MilliGauss_OnThe_XAxis = scaled.XAxis;
  float heading = atan2(raw.YAxis, raw.XAxis);
  
  int xAxis = raw.XAxis;
  int yAxis = raw.YAxis;
  int zAxis = raw.ZAxis;
  

  if(heading < 0)
    heading += 2*PI;
    
  float headingDegrees = heading * 180/M_PI;
  
  Output(raw, scaled, heading, headingDegrees);
}

// last ouput
void Output(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
   Serial.print("Raw:\t");
   Serial.print(raw.XAxis);
   Serial.print("   ");   
   Serial.print(raw.YAxis);
   Serial.print("   ");   
   Serial.print(raw.ZAxis);
   Serial.print("   \tScaled:\t");
   
   Serial.print(scaled.XAxis);
   Serial.print("   ");   
   Serial.print(scaled.YAxis);
   Serial.print("   ");   
   Serial.print(scaled.ZAxis);

   Serial.print("   \tHeading:\t");
   Serial.print(heading);
   Serial.print(" Radians   \t");
   Serial.print(headingDegrees);
   Serial.println(" Degrees   \t");
}

Its show the degree that i want,

3rd step : i setup GPS SKM53

the code are

/*
 This Sketch will run with the SkyNav SKM53 GPS 
 RXD Arduino Pin 3
 TXD Arduino Pin 2
 RST Leave Open 
 NC Leave Open
 GND Ground
 VCC +5
 Make sure you download and save to your Arduino/Libraries folder TinyGPS.h
 and NewSoftSerial.h files.
*/

#include <TinyGPS.h>
#include <SoftwareSerial.h>

unsigned long fix_age;

SoftwareSerial GPS(2,3);
TinyGPS gps;
void gpsdump(TinyGPS &gps);
bool feedgps();
void getGPS();
long lat, lon;
float LAT, LON;

void setup(){
  GPS.begin(9600);
  Serial.begin(9600);
}

void loop(){
  long lat, lon;
  unsigned long fix_age, time, date, speed, course;
  unsigned long chars;
  unsigned short sentences, failed_checksum;

  // retrieves +/- lat/long in 100000ths of a degree
  gps.get_position(&lat, &lon, &fix_age);

  getGPS();
  Serial.print("Latitude : ");
  Serial.print(LAT/100000,7);
  Serial.print(" :: Longitude : ");
  Serial.println(LON/100000,7);
}

void getGPS(){
  bool newdata = false;
  unsigned long start = millis();
  // Every 1 seconds we print an update
  while (millis() - start < 1000)
  {
    if (feedgps ()){
      newdata = true;
    }
  }
  if (newdata)
  {
    gpsdump(gps);
  }
}

bool feedgps(){
  while (GPS.available())
  {
    if (gps.encode(GPS.read()))
      return true;
  }
  return 0;
}

void gpsdump(TinyGPS &gps)
{
  //byte month, day, hour, minute, second, hundredths;
  gps.get_position(&lat, &lon);
  LAT = lat;
  LON = lon;
  {
    feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
  }
}

4th step: i should be able to calculate the bearing and heading from the compass and compare the waypoints then
should make UGV moving.

Here the code :

/*
 This Sketch will run with the SkyNav SKM53 GPS 
 RXD Arduino Pin 3
 TXD Arduino Pin 2
 RST Leave Open 
 NC Leave Open
 GND Ground
 VCC +5
 Make sure you download and save to your Arduino/Libraries folder TinyGPS.h
 and NewSoftSerial.h files.
*/

#include <TinyGPS.h>
#include <SoftwareSerial.h>

unsigned long fix_age;

SoftwareSerial GPS(2,3);
TinyGPS gps;
void gpsdump(TinyGPS &gps);
bool feedgps();
void getGPS();
long lat, lon;
float LAT, LON;

void setup(){
  GPS.begin(9600);
  Serial.begin(9600);
}

void loop(){
  long lat, lon;
  unsigned long fix_age, time, date, speed, course;
  unsigned long chars;
  unsigned short sentences, failed_checksum;

  // retrieves +/- lat/long in 100000ths of a degree
  gps.get_position(&lat, &lon, &fix_age);

  getGPS();
  Serial.print("Latitude : ");
  Serial.print(LAT/100000,7);
  Serial.print(" :: Longitude : ");
  Serial.println(LON/100000,7);
}

void getGPS(){
  bool newdata = false;
  unsigned long start = millis();
  // Every 1 seconds we print an update
  while (millis() - start < 1000)
  {
    if (feedgps ()){
      newdata = true;
    }
  }
  if (newdata)
  {
    gpsdump(gps);
  }
}

bool feedgps(){
  while (GPS.available())
  {
    if (gps.encode(GPS.read()))
      return true;
  }
  return 0;
}

void gpsdump(TinyGPS &gps)
{
  //byte month, day, hour, minute, second, hundredths;
  gps.get_position(&lat, &lon);
  LAT = lat;
  LON = lon;
  {
    feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
  }
}

But its failed, n wont moving. I didnt know where the fault.

You've shown us 4 different sketches. Only the first one has any reference to motors. Which one(s) are you expecting to run the motors? If it's the first one, it's getting no compass or GPS data.

the last code should be able to moving by calculate the bearing from compass and GPS. But its not working

hafidah:
the last code should be able to moving by calculate the bearing from compass and GPS. But its not working

Well, it’s not working because it doesn’t have any code in it to move the motors. Nor does it have any compass code. If you want it to move the motors, it NEEDS code to do it. That’s not a fault. It’s just not a program that does what you want it to do.

I am sorry this is the real 4th code as per attachment

waypointlatest3AB.ino (10.3 KB)

The question here, based from calculation should we able to move the UGV to certain waypoint, but it didnt work,

Anyone please help.... =( =( =(

but it didnt work

That presumably means it didn't do what you expected, but it did do something.
You haven't told us what that thing was.
What does your debug output look like?

from the serial monitor, its show GPS location :
lat: 000000000 long: 0000000. thats all

So, you're somewhere off the west coast of Africa.
What do your debug prints tell you about the state of the GPS?
Of the compass?

i already setup the compass (code #2) n GPS(code #3) but seperately.

its working just fine

The problem is when they all combine together, its failed to work auto

regards,

hafidah

The problem is when they all combine together, its failed to work auto

But what do your debug prints tell you is going on?
Just bleating that it doesn't work isn't going to fix it.