Return to home boat rc

Good evening, I have a boat (I will call it RC) in which I have put gps with the help of the arduino. The movements in the RC are done with the following: 1) motor only for front and 2) servo for rudder right left. So we have 3 directions in which we can move the RC (front / right / left). When powering up the system, it records the boot location in a variable. Now during its course in the sea it checks and stores again in a variable its current location. So let’s turn the GPS switch from OFF to ON from the remote control. When this is done, the switch goes from OFF to ON and records the GPS activation location in a 3rd variable. So we have:

StartUp Position
BaseStartX- BaseStartY (Recorded 1 time)

GPS Activation Variables
GPSStartX - GPSStartY (Recorded 1 time)


And the current Location of the second
ActiveX -ActiveY (refreshed every second)

The question: To turn the RC from location (xB, yB) to location (xA, yA), everything is known. What is not known is when activating the GPS that the boat “looks” in relation to the starting point, ie does it have our back? does he have a face? does he have a profile for us? I think the first step is to turn it to the starting point and then adjust it either to the right or to the left to follow the imaginary straight line of the return.

My code is

#include <NeoSWSerial.h>
#include <Servo.h>
#include <TinyGPS++.h>

static const int RXPin = 8;
static const int TXPin = 9;
static const int LEDGPSPin = 12;
static const int SWITCHGPSPin = 11;
static const int servoPin = 10;

static const uint32_t GPSBaud = 9600;

TinyGPSPlus gps;

NeoSWSerial ss(RXPin, TXPin);
Servo Servo1;

float BaseStartY;
float BaseStartX;
bool ReadBasePosition;

float GPSStartY;
float GPSStartX;
bool ReadGPSPosition;

float PreviousX;
float PreviousY;

float ActiveX;
float ActiveY;


void setup()
{
 Serial.begin(9600);
 ss.begin(GPSBaud);
 
 pinMode(LEDGPSPin, OUTPUT);
 pinMode(4, OUTPUT);
 pinMode(5, OUTPUT);
 pinMode(SWITCHGPSPin, INPUT_PULLUP);
 
 ReadBasePosition=false;
 ReadGPSPosition=false;

 Servo1.attach(servoPin); 
}
int ZeroPoint=90;
int moires=90;
int turnMS=5;
int pos=0;


void loop()
{
  gpsFunc();
}

void gpsFunc()
{
while (ss.available() > 0)
 {
   gps.encode(ss.read());
   if (gps.location.isUpdated())
   {
     
     digitalWrite(LEDGPSPin, HIGH);
      
     ActiveX=gps.location.lng();
     ActiveY=gps.location.lat();
    
     
     if (!ReadBasePosition)
     {
       
        BaseStartX=ActiveX;
        BaseStartY=ActiveY;
        ReadBasePosition=true;
        Serial.println("READ BASE POSITION");
     }
     else
     {
       if (digitalRead(SWITCHGPSPin)==HIGH) //GPS SWITCH TO RETURN TO HOME ACTIVED
       {
        
          if (!ReadGPSPosition)
          {
            GPSStartX=ActiveX;
            GPSStartY=ActiveY;
            ReadGPSPosition=true;
            Serial.println("READ GPS POSITION");
          }
          else
          {
            //THIS IS THE POSITION OF CODE THAT MISSING TO TURN THE BOAT LEFT/RIGHT OR MID INDEPENCE VARIABLES THAT WE HAVE!
			// WE HAVE:
			// BaseStartX,BaseStartY (The position that the system startup)
			// GPSStartX,GPSStartY (the position that Return to home activated)
			// PreviousX,PreviousY (the possition of previous loop)
			// ActiveX,ActiveY (the current position)
			
			//I'ME SURE THAT WITH THESE VARIABLES ARE ENOUGH TO RETURN THE RC BACK TO BaseStartX,BaseStartY.
			//THAT IS THE QUENSTION!
		  }
       }
       else
       {
         Serial.println("GPS DEACTIVED");
         ReadGPSPosition=false;
       }
     }
     PreviousX=ActiveX;
     PreviousY=ActiveY;
     delay(1000);
   }
 }
 digitalWrite(LEDGPSPin, LOW);
}



//----------------------------------------------------------------------------------
void ServoRight()
{
  
  int deksia=moires*2;
  if (pos==moires)
  {
    for (pos = moires; pos <deksia; pos+=1) 
    { 
      Servo1.write(pos);             
      delay(turnMS);                      
    }
  }
  else if (pos==(ZeroPoint-moires))
  {
    for (pos = ZeroPoint-moires; pos <deksia; pos+=1) 
    { 
      Servo1.write(pos);             
      delay(turnMS);                      
    }
  }
}
void ServoLeft()
{
  if (pos==moires)
  {
    for (pos = moires; pos >(ZeroPoint-moires); pos-=1) 
    { 
      Servo1.write(pos);             
      delay(turnMS);                      
    }
  }
  else if (pos==moires*2)
  {
    for (pos = moires*2; pos >(ZeroPoint-moires); pos-=1) 
    { 
      Servo1.write(pos);             
      delay(turnMS);                      
    }
  }
}
void ServoMid()
{
  if (pos==moires*2)
  {
    for (pos = moires*2; pos >moires; pos-=1) 
    { 
      Servo1.write(pos);             
      delay(turnMS);                      
    }
  }
  else if (pos==(ZeroPoint-moires))
  {
    for (pos = ZeroPoint-moires; pos <moires; pos+=1) 
    { 
      Servo1.write(pos);             
      delay(turnMS);                      
    }
  }

}

A compass is what you are lacking.

Paul

If the GPS is operating and the boat is moving under power, it will tell you your heading. You just need to turn until the heading points to the start point.

TinyGPS has a function that will calculate the bearing from the current GPS location to any GPS endpoint ("home" if you like).

Use that bearing in conjunction with the GPS heading, as mentioned in the above reply. However, a compass heading will be much more accurate than the GPS heading.

wildbill:
If the GPS is operating and the boat is moving under power, it will tell you your heading. You just need to turn until the heading points to the start point.

jremington:
TinyGPS has a function that will calculate the bearing from the current GPS location to any GPS endpoint ("home" if you like).

Use that bearing in conjunction with the GPS heading, as mentioned in the above reply. However, a compass heading will be much more accurate than the GPS heading.

Interesting answers, i cant find any example with tinygps that do that!

Paul_KD7HB:
A compass is what you are lacking.

Paul

i use arduino uno and GY-NEO6MV2. Can i do that? And if yes how?

achillf:
i use arduino uno and GY-NEO6MV2. Can i do that? And if yes how?

Try a Google search on 'Arduino Compass'

Several tutorials to be found on the Internet.

Hi,
Look at this TinyGPS++ example;

#include <TinyGPS++.h>
#include <SoftwareSerial.h>
/*
   This sample code 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 = 4, TXPin = 3;
static const uint32_t GPSBaud = 4800;


// The TinyGPS++ object
TinyGPSPlus gps;


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


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


  Serial.println(F("FullExample.ino"));
  Serial.println(F("An extensive example of many interesting TinyGPS++ features"));
  Serial.print(F("Testing TinyGPS++ library v. ")); Serial.println(TinyGPSPlus::libraryVersion());
  Serial.println(F("by Mikal Hart"));
  Serial.println();
  Serial.println(F("Sats HDOP  Latitude   Longitude   Fix  Date       Time     Date Alt    Course Speed Card  Distance Course Card  Chars Sentences Checksum"));
  Serial.println(F("           (deg)      (deg)       Age                      Age  (m)    --- from GPS ----  ---- to London  ----  RX    RX        Fail"));
  Serial.println(F("----------------------------------------------------------------------------------------------------------------------------------------"));
}


void loop()
{
  static const double LONDON_LAT = 51.508131, LONDON_LON = -0.128002;


  printInt(gps.satellites.value(), gps.satellites.isValid(), 5);
  printFloat(gps.hdop.hdop(), gps.hdop.isValid(), 6, 1);
  printFloat(gps.location.lat(), gps.location.isValid(), 11, 6);
  printFloat(gps.location.lng(), gps.location.isValid(), 12, 6);
  printInt(gps.location.age(), gps.location.isValid(), 5);
  printDateTime(gps.date, gps.time);
  printFloat(gps.altitude.meters(), gps.altitude.isValid(), 7, 2);
  printFloat(gps.course.deg(), gps.course.isValid(), 7, 2);
  printFloat(gps.speed.kmph(), gps.speed.isValid(), 6, 2);
  printStr(gps.course.isValid() ? TinyGPSPlus::cardinal(gps.course.deg()) : "*** ", 6);


  unsigned long distanceKmToLondon =
    (unsigned long)TinyGPSPlus::distanceBetween(
      gps.location.lat(),
      gps.location.lng(),
      LONDON_LAT, 
      LONDON_LON) / 1000;
  printInt(distanceKmToLondon, gps.location.isValid(), 9);


  double courseToLondon =
    TinyGPSPlus::courseTo(
      gps.location.lat(),
      gps.location.lng(),
      LONDON_LAT, 
      LONDON_LON);


  printFloat(courseToLondon, gps.location.isValid(), 7, 2);


  const char *cardinalToLondon = TinyGPSPlus::cardinal(courseToLondon);


  printStr(gps.location.isValid() ? cardinalToLondon : "*** ", 6);


  printInt(gps.charsProcessed(), true, 6);
  printInt(gps.sentencesWithFix(), true, 10);
  printInt(gps.failedChecksum(), true, 9);
  Serial.println();
  
  smartDelay(1000);


  if (millis() > 5000 && gps.charsProcessed() < 10)
    Serial.println(F("No GPS data received: check wiring"));
}


// This custom version of delay() ensures that the gps object
// is being "fed".
static void smartDelay(unsigned long ms)
{
  unsigned long start = millis();
  do 
  {
    while (ss.available())
      gps.encode(ss.read());
  } while (millis() - start < ms);
}


static void printFloat(float val, bool valid, int len, int prec)
{
  if (!valid)
  {
    while (len-- > 1)
      Serial.print('*');
    Serial.print(' ');
  }
  else
  {
    Serial.print(val, prec);
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // . and -
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
    for (int i=flen; i<len; ++i)
      Serial.print(' ');
  }
  smartDelay(0);
}


static void printInt(unsigned long val, bool valid, int len)
{
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i=strlen(sz); i<len; ++i)
    sz[i] = ' ';
  if (len > 0) 
    sz[len-1] = ' ';
  Serial.print(sz);
  smartDelay(0);
}


static void printDateTime(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial.print(F("********** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial.print(sz);
  }
  
  if (!t.isValid())
  {
    Serial.print(F("******** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second());
    Serial.print(sz);
  }


  printInt(d.age(), d.isValid(), 5);
  smartDelay(0);
}


static void printStr(const char *str, int len)
{
  int slen = strlen(str);
  for (int i=0; i<len; ++i)
    Serial.print(i<slen ? str[i] : ' ');
  smartDelay(0);
}

Look at the London references, these are where headings are calculated.
Course and distance.

Tom… :slight_smile:

The TinyGPS function courseto() gives you the heading.

All answers untill now was really helpfull. But i cant solve problem exactly. Here is the next step of my code:

//IN FIRST LOOP I LOCATE IN VARIABLES THE BASE POSITION (ONE TIME ON THE LOOP)
// BaseStartX,BaseStartY

//WHEN ACTIVE RETURN TO HOME I LOCATE IN VARIABLE THE GPS ACTIVETED POSITION (ONE TIME ON THE LOOP)
// GPSStartX,GPSStartY

//ALSO I LOCATE Distance between BASE and GPS ACTIVETED Position at once (ONE TIME ONE THE LOOP)
//  DistanceBaseGPS=(unsigned long)TinyGPSPlus::distanceBetween(GPSStartY,GPSStartX,BaseStartY,BaseStartX);//1.1


LOOP EVERY SECOND
{
ActiveX=gps.location.lng();
ActiveY=gps.location.lat();
DistanceBaseActive=(unsigned long)TinyGPSPlus::distanceBetween(ActiveY,ActiveX,BaseStartY,BaseStartX);//2.1
 
double courseToDestination = TinyGPSPlus::courseTo(ActiveY, ActiveX, BaseStartY, BaseStartX);
const char *directionToDestination = TinyGPSPlus::cardinal(courseToDestination);
int courseChangeNeeded = (int)(360 + courseToDestination - gps.course.deg()) % 360;
			  
			  
              if (DistanceBaseActive>DistanceBaseGPS)
              {
                 ServoRight();
              }
              else
              {
                if ((courseChangeNeeded >= 345) || (courseChangeNeeded < 15)) 
                {
                  ServoMid();
                }
                else if (CourseBaseActive>CourseBaseGPS)
                {
                  ServoRight();
                }
                else if (CourseBaseActive<CourseBaseGPS)
                {
                  ServoLeft();
                }
         
              }
}

My thinking is and correct me if i was wrong.
If distance between active position and base is larger than gps position and base
then turn right
Do this in every loop until this condition give opposite result
With this condition im trying to turn the rc in the return’s position. Is this right method??

Else
turn right or left indepence the differences between courses of the BasePosition-GPSPosition with ActivePosition-BasePosition.

The correct active course value must be near or even same with the course of the Base-Gps course.

To drive towards a goal, most people compare the current direction of travel (the heading) with the desired direction of travel (the bearing). The TinyGPS courseto() function can give you the bearing from the current position to the goal.

if (heading - bearing) > 0 turn left a small amount
if (heading - bearing) < 0 turn right a small amount
if (heading == bearing) continue straight

I Understand exactly your conditions and i agree with you. Then only thing that remaining is that before your conditions must turn the rc to the right way, must go direct back to base. With only these 3 conditions could be senario to go away but in the straight line of two positions. We dont want that. We must turn first and the use your conditions.

The picture on the right is wrong.

Bearing and heading are each numbers between 0 and 360 degrees, and there is no possible confusion leading to travel in a direction 180 degrees away from the goal.

This short video shows the method in action (especially, for the last leg of the course): http://www.uoxray.uoregon.edu/orangutan/boat2.mp4

jremington:
The picture on the right is wrong.

Bearing and heading are each numbers between 0 and 360 degrees, and there is no possible confusion leading to travel in a direction 180 degrees away from the goal.

This short video shows the method in action (especially, for the last leg of the course): http://www.uoxray.uoregon.edu/orangutan/boat2.mp4


Im still dont know where is my mistake, can you explain me with code?? Yes i want exactly what rc does on video.

In real life, most people use PID Control to determine the “small amount” of turn.

The boat shown in the video has differential drive (two motors, one right and one left), so turns are accomplished by varying the two motor speeds. The boat uses a BNO055 3D sensor to determine the current heading.

Here is the Arduino code for the actual steering shown in the video.

// continue to run this leg, steering with IMU
// (may not have recent GPS info in the following)

		I2C_Read3Vectors(BNO055_A0,BNO055_FUSED_EULER, (int16_t *)v);  //get current Euler angles

		for(i=0; i<3; i++) v[i] >>= 4;  //divide by 16 to get degrees
		imu_heading = wrap360(v[0] + yaw_offset); //yaw angle is v[0], correct for magnetic declination

                error = heading_error(point_bearing, imu_heading);
		
		//error is positive if current_heading > bearing (compass direction)
		//positive bias acts to reduce left motor speed, so bear left

		bias = (kp*error)/10;  //Kp in tenths (Ki, Kd not necessary in water)

		// the motor routines internally limit the argument to {-255, 255}

		set_m1_speed(motorSpeed-bias); //left motor
		set_m2_speed(motorSpeed+bias); //right motor

// check for recent GPS fix

The problem still remaining, this code is not usefull because i dont have BNO055 3D sensor. The first step to do is to get boat to the corrent position to return. If we are sure that the boat “looks” in the base position then we can use your condition with courses.

The code does not depend on which type of sensor or method is used to get the heading.

Good luck with your project!