GPS Guided Vehicle

5
April
2018

Here is the code which I have used for my GPS Guided vehicle. I have used GY-273 HMC5883L Module Triple Axis Compass Magnetometer Sensor Module as Compass, NEO6M GPS module and an L298N motor driver for driving my motors of the vehicle. I have used an Arduino UNO for the said purpose. The code is self-explanatory with the comments describing the respective codes. Please do comment if you want me to put up a full tutorial or you have any trouble.

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
#include <Wire.h>
#include <HMC5883L.h>
#include <TinyGPS++.h>
#include <SoftwareSerial.h>

HMC5883L compass;

static const int RXPin = 4, TXPin = 3;
static const uint32_t GPSBaud = 9600;

// The TinyGPS++ object
TinyGPSPlus gps;

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

//FOR DRIVING THE MOTOR I'M USING L298N MOTOR DRIVER
//ALSO I'M NOT USING PWM PINS FOR ANY CONTROL OF SPEED OF MOTOR

// motor one
int in1 = 5;
int in2 = 6;
// motor two
int in3 = 9;
int in4 = 10;

//DECLARING CURRENT AND TARGET LATITUDE AND LONGITUDE
//DECLARING CURRENT AS 1 FOR CHECKING PURPOSE. MOTORS WILL NOT START UNLESS THE VALUE OF CURRENT POSITION IS CHANGED FROM 1 TO ACTUAL COORDINATES
float clat = 1;
float clon = 1;
float tlat;
float tlon;

void setup()
{
  //FOR MOTORS
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  Serial.begin(9600);
  ss.begin(GPSBaud);
  
  // Initialize Initialize HMC5883L
  Serial.println("Initialize HMC5883L");
  while (!compass.begin())
  {
    Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
    delay(500);
  }

  // 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()
{
  Vector norm = compass.readNormalize();

  // Calculate heading
  float 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);
  float 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
  float headingDegrees = heading * 180/M_PI; //ACTUAL DIRECTION WHERE THE VEHICLE POINTS

  delay(100);
  
  while (ss.available() > 0){
    gps.encode(ss.read());
    if (gps.location.isUpdated()){
      
      //SETTING CURRENT LATITUDE AND LONGITUDE
      
      clat = gps.location.lat(); //home
      clon = gps.location.lng(); //home 
    }
  }

  //BEARING
  //WHERE THE VEHICLE HAS TO ACTUALLY POINT WHILE MOVING
  //TARGET POSITION OF VEHICLE
  
  tlat = 18.69849;
  tlon = 81.25036;
  float lat1 = radians(clat); //home
  float lon1 = radians(clon); //home
  float lat2 = radians(tlat); //target
  float lon2 = radians(tlon); //target
  float y = sin(lon2-lon1) * cos(lat2);
  float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2-lon1);
  float bearing = atan2(y, x);
  if(bearing<0) bearing+=TWO_PI;
  bearing = degrees(bearing);
  Serial.println(bearing);
  Serial.println(headingDegrees);
  
  //DISTANCE
  //CALCULATION OF ACTUAL DISTANCE TO TARGET
  float R = 6371000;
  float delta_lat = radians(tlat-clat);
  float delta_lon = radians(tlon-clon);
  float a = (sin(delta_lat/2) * sin(delta_lat/2)) + (cos(lat1) * cos(lat2) * sin(delta_lon/2) * sin(delta_lon/2));
  float c = 2 * atan2(sqrt(a), sqrt(1-a));
  float d = R * c;
  
  float target = bearing;
  float diff = target-headingDegrees; //TO KNOW WHEN TO TURN AND IN WHICH DIRECTION
  
  if(diff >= 5 && d > 2 && clat!=1) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);  
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW); 
  } else if(diff <= -5 && d > 2 && clat!=1)  {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);  
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);     
  } else if(diff > -5 && diff < 5 && d > 2  && clat!=1)  {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);  
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);  
  } else  {
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);  
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
  }
}


By administrator at 12:11:58 PM 0 Comment(s)

Comments

There are no comments yet. Be the first one to add a comment!

Add a Comment

Please enter the email address.Invalid format. (Won't be Displayed)
Notify me of followup comments
Enter the displayed Code: captcha