I've created a working code for an autonomous rc car that uses GPS as well as a compass to move to predetermined waypoints. Recently I discovered NeoGPS and it is a very powerful system. However, I cannot fully implement the code into my existing code. Mainly trying to figure out how get the code to work first without the compass being used, and only GPS.
I need a way to get the car to move by having the GPS tell the car to move forward, left, or right if it begins to veer off course in either direction.
/*
Name: MoBot.ino
Created: 8/15/2017 10:56:15 AM
Author: Mad Monkey Projects
*/
// ARDUINO (UNO) SETUP:
// =======================
// Ping sensor = 5V, GRND, D11 (for both trigger & echo)
// Adafruit GPS = D8 & D9 (GPS Shield, but pins used internally)
// Adafruit Magnetometer Adafruit HMC5883L = I2C : SCL (A5) & SDA (A4)
// SD Card (D10, D11, D12, D13)
#include <NMEAGPS.h>
#include <AltSoftSerial.h>
#include <Wire.h>
#include "MotorDriver.h"
#include "Compass.h" // written by https://github.com/helscream/HMC5883L_Header_Arduino_Auto_calibration
// requires specific pins (.g., 8&9 on an UNO):
AltSoftSerial gpsPort(8, 9); // baud rate must be 9600, 19200 or 38400;
/******************************
* Pin definition for motors *
******************************/
// drive motor
constexpr uint8_t ENA = 5;
constexpr uint8_t IN1 = 4;
constexpr uint8_t IN2 = 3;
// steering motor
constexpr uint8_t ENB = 6;
constexpr uint8_t IN3 = 14;
constexpr uint8_t IN4 = 15;
// motor instance
MotorDriver Drive(ENA, IN1, IN2);
MotorDriver Turn(ENB, IN3, IN4);
/******************************
* GPS information *
******************************/
NMEAGPS gps;
gps_fix fix;
/******************************
Waypoint information
******************************/
// the vehicle will stop when currentTarget is equal to number of coordinates.
constexpr uint8_t NUM_OF_COORDINATES = 4;
// enter waypoints
NeoGPS::Location_t TARGET_WAYPOINTS[] =
{ /* LAT LON */
{ 348326564, -922737374L },
{ 348328046, -922737897L },
{ 348327593, -922738434L },
{ 348325934, -922735979L }
};
float CurrentLat, CurrentLong;
/* CurrentTarget keeps track of the target in the array. The heading threshold is the amount of degrees
the vehicle can be off its desired heading before adjusting the course. Distance threshold is the amount
of meters away from target coordinate, that when exceeded it will continue towards the next coordinate. */
uint8_t CurrentTarget = 0, HeadingThreshold = 5, DistanceThreshold = 2;
// the distance to the current target coordinate is calculated
float Distance = fix.location.DistanceKm(TARGET_WAYPOINTS[CurrentTarget]);
// calculates the desired/"optimal" heading based on the current location and target location
float TargetHeading = fix.location.BearingToDegrees(TARGET_WAYPOINTS[CurrentTarget]);
void setup()
{
Serial.begin(115200);
gpsPort.begin(9600);
// configure GPS
gpsPort.println(F("$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n" // RMC only
"$PMTK220,100*2F\r\n" // 10 Hz update
"$PGCMD,33,0*6D")); // no antenna
// used to improve accuracy of HMC5883L compass
// found by running the calibration functions supplied by the library
Wire.begin();
compass_x_offset = -199.89;
compass_y_offset = -70.435;
compass_z_offset = -716.97;
compass_x_gainError = 0.97;
compass_y_gainError = 1.01;
compass_z_gainError = 1.00;
// intial speed and direction
Drive.setSpeed(255);
Drive.forward();
}
void loop()
{
Serial.println("----------------------------------------------");
compass_heading();
heading += 0.017;
GrabGPSData();
AdjustCourse();
Serial.print("Compass Heading: ");
Serial.println(heading);
Serial.println("----------------------------------------------");
}
void GrabGPSData()
{
gps.available(gpsPort);
gps_fix fix = gps.read();
if (fix.valid.location) {
CurrentLat = fix.latitudeL();
CurrentLong = fix.longitudeL();
//Serial.print("Current Location: ");
//Serial.print(CurrentLat, 7);
//Serial.print(", ");
//Serial.println(CurrentLong, 7);
}
else {
//Serial.println("No GPS fix!");
}
return;
}
//The function for adjusting the course of the vehicle
//Based on the current heading and desired heading
void AdjustCourse()
{
// stops the vehicle when there are no coordinates left
// if the distance threshold is exceeded, move on to the next target location in the array
if(CurrentTarget == NUM_OF_COORDINATES){
//Serial.println("No coordinates left in the array.");
}
if(Distance < DistanceThreshold){
CurrentTarget++;
}
//Serial.print("Distance to next coordinate: ");
//Serial.println(Distance);
// prints the target location
//Serial.print("Target Location(");Serial.print(CurrentTarget);Serial.print("): ");
//Serial.print("Target heading: ");
//Serial.print(TargetHeading);
// find the difference between the current heading and the target heading
// does some correction to find the correct error
int16_t Error = TargetHeading - heading;
if(Error < -180) Error += 360;
if(Error > 180) Error -= 360;
//Serial.print(" - Error: ");
//Serial.print(Error);
// depending on the error, the vehicle will then adjust its course
// small delays for the left and right turns are added to ensure it doesn't oversteer.
if(abs(Error) <= HeadingThreshold) {
//Serial.println(" - On course");
Drive.setSpeed(255);
Drive.forward();
Turn.release();
}
else if(Error < 0) {
//Serial.println(" - Adjusting towards the left");
Turn.setSpeed(255);
Turn.left();
}
else if (Error > 0) {
//Serial.println(" - Adjusting towards the right");
Turn.setSpeed(255);
Turn.right();
}
else {
Drive.setSpeed(255);
Drive.forward();
Turn.release();
}
}
MoBot_v0.4.ino (5.25 KB)