Here is my sketch:
________________________
//Nemo fishing torpedo. By Noel Grant
#include <NewSoftSerial.h>
#include <LiquidCrystal.h>
#include <nmea.h>
#include <PWMServo.h> // The other servo library conflicts with newsoft serial
#undef round
NewSoftSerial nss(2, 3);
PWMServo steerservo;
PWMServo speedservo;
NMEA gps(GPRMC); // GPS data connection to GPRMC sentence type
int d; // relative direction to destination
int ledn=0; //navigation LEDS
int lednPin = 8; // navigation LEDS at pin 8
//int potpin = 0; // analog pin used to connect the potentiometer
int releasePin=3;
int ol=6; //overload is High on pin 6
int lv =11; // low voltage detect on pin 11
int dis; // variable to read the value from the analog pin
int soff; // steering offset
int lvr; // low volts in return leg (so it only flashes the LEDs once in the loop)
long mst; // motor start time
//int compin=1; // Wiper of compas potentiometer connected to analogue pin A1
float comrad;
float wp1lat; // for the calculated waypoint latitude
float wp1lon; // for the calculated waypoint longitude
float lat, lon;
float hlat; //home latitude
float hlon; // home longitude
float Home_latitude;
float Home_longitude;
float dest_latitude;
float dest_longitude;
float distance;
int pset=0; // set home position once only
int k=0; // used in a while loop
int wptRange; //Set to desired waypoint radius in meters
int heading;
float speed;
int fast; // running too fast >8km/hr
int slow; //running too slow <6km/hr
int faster=0;
int slower=0;
int sterof =0;
float ct=90; // set the motor power at start.. 90 is the mid point (for Pololu controller)
//set the start time once only
int coma; //adjusted compas heading
LiquidCrystal lcd(16, 15, 5, 4, 13, 12);
int stay=0; // Stay mode when this =1
//long stayt; // timer for stay period
//int slc =0; // stay loop counter to ensure stay timer is only set the first time thru the loop.
int z=0;
//int sto=0; // Stay mode Time Out
void setup()
{lcd.begin(16, 2); lcd.setCursor(0, 1); // set cursor to column zero line 1
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
lcd.setCursor(1, 1);lcd.print("NEMO DM 1 2016"); delay(5000);
speedservo.attach(10); //speed servo on pin 10
soff=0; // *************steering offset ***********
steerservo.attach(9);
steerservo.write(90+soff); //+3 DEGREES TO CORRECT FOR BOAT RUDDER
pinMode(releasePin,OUTPUT);
digitalWrite(3,HIGH); lcd.setCursor(0, 0);lcd.print("Release open"); //Open release servo to attach line
delay(5000); // hold release servo open for 5 seconds
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
lcd.setCursor(1, 1);lcd.print("Release closed");
digitalWrite(3,LOW); //Close release servo
delay(3000);
pinMode(lednPin, OUTPUT); digitalWrite(8,HIGH); // LEDs off
pinMode(11,INPUT); // low V detetection
pinMode(6,INPUT); // pin 6 input for overload
int lvr=0; // RETURN IF VOLTAGE LOW
Serial.begin(9600);
delay(10);
nss.begin(9600);
delay(10);
//Determine the MODE
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
dis= analogRead(4);
dis= map(dis, 0, 1023, 0, 1000); // scale distance pot to 1500 meters
if (dis<50) {stay =1;} else {stay=0;} // way of recognising that stay mode is required. Done once before the travel distance is set.
// READ & SET DISTANCE
for(int e=0; e<300; e++)
{delay(50); // delay 10 seconds to set compass reading
dis= analogRead(4);
dis= map(dis, 0, 1023, 0, 1000); // scale distance pot to 1000 meters (reduced from 1500m on 030116)
if (dis<200) {dis =200;} // Set minimum distance
lcd.setCursor(0, 0);lcd.print("Distance ");lcd.setCursor(1, 1);lcd.print(dis);lcd.print(" metres");}
// READ & SET COMPAS
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
for(int e=0; e<300; e++)
{delay(50);
coma= analogRead(4);
coma=map(coma, 0, 1023, 0, 359);
lcd.setCursor(0, 0);lcd.print("Compas reading ");lcd.setCursor(1, 1);lcd.print(coma);lcd.print(" degrees");}
wptRange=dis/20;
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
lcd.setCursor(0, 0);lcd.print("Waiting GPS lock");
if (stay==1) {lcd.setCursor(1,1);lcd.print("Stay mode");} else {lcd.setCursor(1,1);lcd.print("Return mode"); }
delay(5000); }
//******************* end of setup ******************************************* END OF SETUP ****************************************
void loop()
{
do {digitalWrite(8,HIGH);
if (nss.available() > 0 ) {
char c = nss.read();
// check if the character completes a valid GPS sentence
if (gps.decode(c)) {
// check if GPS positioning was active
digitalWrite(8,LOW);delay(50);digitalWrite(8,HIGH);delay(50);digitalWrite(8,LOW);delay(50);digitalWrite(8,HIGH);delay(50);digitalWrite(8,LOW);}}}
while (gps.gprmc_status() != 'A');
hlat=gps.gprmc_latitude(); // SET THE HOME LAT & LON
hlon=gps.gprmc_longitude();
//Serial.print("Home lat "); Serial.print(hlat,7); Serial.print(", Home lon "); Serial.println(hlon,7);
//Serial.println("Setting the WP co ordinates");
Home_latitude = hlat * PI/180; // Start lat and lon (from GPS) converted into radian
Home_longitude = hlon * PI/180;
comrad = coma * PI/180; // Compass heading converted into radians
distance = dis; //distance to be traveld in meters
wp1lat = asin(sin(Home_latitude) * cos(distance/6371580) + cos(Home_latitude)*sin(distance/6371580)*cos(comrad)); //6371580 IS EARTH RADIUS AT NZ
wp1lon = Home_longitude + atan2(sin(comrad)*sin(distance/6371580)*cos(Home_latitude),cos(distance/6371580)-sin(Home_latitude)*sin(wp1lat));
wp1lat = wp1lat * 180.0/PI; // convert back into degrees for Navigation functions
wp1lon = wp1lon * 180.0/PI;
//lcd.setCursor(0, 1);lcd.print("Home = "); delay(1000); lcd.setCursor(0, 1);lcd.print(hlat,7);lcd.setCursor(1,1);lcd.println(hlon,7);delay(2000);
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
lcd.setCursor(0, 0);lcd.print("waypoint = "); delay(1000);lcd.setCursor(0, 0);lcd.print(wp1lat,7); lcd.setCursor(1, 1);lcd.print(wp1lon,7); delay(2000);
Serial.print("wp = "); Serial.print(wp1lat,7); Serial.print(",");Serial.println(wp1lon,7);
delay(3000);
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
lcd.setCursor(0, 0);lcd.print("Launch now");
//Serial.println("");
//Serial.println("=============================");
k=1; // While k=1 steer to waypoint
speedservo.write(150);// START THE MOTOR AT FULL POWER
mst=millis();
//***************************************** END OF WAYPOINT CALCULATIONS ********************************** END OF WAYPOINT CALCULATIONS ****************
do {
if (nss.available() > 0 ) {
char c = nss.read();
if (gps.decode(c)) {
if (gps.gprmc_status() == 'A')
{
d = gps.gprmc_course_to(wp1lat,wp1lon) - gps.gprmc_course(); // steer for waypoint
if (ledn==0) {ledn=1; digitalWrite(8,LOW);delay(20);} //navigation LED
if ((gps.gprmc_speed(KMPH)< 0.5) && (millis()<(mst+240000))) {steerservo.write(95);sterof=1;} // Steer straight (with slight left bias) until there is some speed attained.
//Only in the first 4 minutes.
if (d < 0) { d += 360; } //convert d to + - 180 degrees
if (d > 180) { d -= 360; }
//Serial.print("d= ");Serial.println(d);
if (d>-1 && d<1 && (sterof==0)) {steerservo.write(90+soff);} // + is turn right 90 degrees is straight ahead
if (d>=2 && (sterof==0)) {
steerservo.write(85+soff);}
if (d>5&& (sterof==0)) {
steerservo.write(80+soff);}
if (d>10&& (sterof==0)) {
steerservo.write(65+soff);}
if (d>20&& (sterof==0)) {
steerservo.write(40+soff);}
if (ledn==1) {ledn=0; digitalWrite(8,HIGH);}
if (d<=-2&& (sterof==0)) {
steerservo.write(95+soff);}
if (d<-5&& (sterof==0)) {
steerservo.write(100+soff);}
if (d<-10&& (sterof==0)) {
steerservo.write(115+soff);}
if (d<-20&& (sterof==0)) {
steerservo.write(140+soff);}
sterof=0;
//speedservo.write(150); // full power on all of outward leg
int ol=digitalRead(6);
if (ol==1) {speedservo.write(90);
delay(2000);speedservo.write(10);delay(3000);speedservo.write(90);delay(2000);speedservo.write(150); digitalWrite(8,HIGH);
delay(5000);
for (int i=0; i<10; i++)
{digitalWrite(8,LOW); // led on for morse character A for Amp overload (dot dash)
delay (100);
digitalWrite(8,HIGH); //end of dot
delay(100); // led off inter element gap
digitalWrite(8,LOW); delay(300);// led on for dash
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
digitalWrite(8,HIGH);delay(600);}lcd.setCursor(0, 0);lcd.print("Current overload");}
//if motor overload stop motor, wait 2 sec then reverse motor 3 sec,wait 2 sec, then go forward 5 second delay to allow battery V to recover after overload so
//LV sense wont go off
ol=0;
int lv =digitalRead(11); // if voltage low flash LEDs, drop line & return home
if (lv==1) //********************************* 20 minute timer or low V on outward leg trigger abort mission
{for(int i=0; i<10; i++)
{digitalWrite(8,LOW);delay(100); // first dot of character V
digitalWrite(8,HIGH);delay(100); //inter
digitalWrite(8,LOW);delay(100); // 2nd dot
digitalWrite(8,HIGH);delay(100); //inter
digitalWrite(8,LOW);delay(100); // 3rd dot
digitalWrite(8,HIGH);delay(100); // inter
digitalWrite(8,LOW);delay(300); // dash
digitalWrite(8,HIGH);delay(600);} // charater end
lcd.setCursor(1, 1);
lcd.print("Low V outward");
k=2; } //signal V then abort mission
//*************************************
// TIMEOUT when in return mode (stay=0)
z=0;
if (millis()>(mst+1500000)) {z=1;} // Motor start time plus fixed time (25 MINUTE DELAY) IN RETURN MODE ONLY.
if (z==1 && stay==0) {for(int i=0; i<10; i++)
{digitalWrite(8,LOW);delay(300);digitalWrite(8,HIGH);delay(500);} //Morse character T one dash
k=2;} //fast flash LED then abort mission
if (k==2) {digitalWrite(3,HIGH);delay(5000);digitalWrite(3,LOW); } //signal to operate release servo for 5 seconds
z=0;
//************************************
// IS IT IN THE ZONE for return mode
if (gps.gprmc_distance_to(wp1lat,wp1lon,MTR) < wptRange) {z=2;}
if (z==2 && stay==0) // See If its in the zone. IN RETURN MODE ONLY
{k=2;}
if (k==2) {digitalWrite(3,HIGH);delay(5000);digitalWrite(3,LOW); } //signal to operate release servo for 5 seconds
z=0;
//**********************************
//IS IT IN THE ZONE FOR STAY MODE (time base as distance based did not work out)
if (gps.gprmc_distance_to(wp1lat,wp1lon,MTR) < wptRange) {z=3;} else {z=4;}
if (z==3 && stay==1) {speedservo.write(90); for(int i=0; i<20; i++) // stop motor
{digitalWrite(8,LOW);delay(100);
digitalWrite(8,HIGH);delay(400);
digitalWrite(8,LOW);delay(100);
digitalWrite(8,HIGH);delay(400);
digitalWrite(8,LOW);delay(100);}} // delay 20 seconds to allow time to drift while keeping LED flashing
if (z==4 && stay==1) {speedservo.write(150);} // OUTSIDE THE ZONE SO START MOTOR
//lcd.setCursor(1, 1);lcd.print("z= ");lcd.print(z);
//********************************
// WHEN TO END STAY MODE
if (lv==1) {lcd.setCursor(0, 0);
lcd.print("Low V ended stay"); k=3;}
//*********************************
//lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
//lcd.setCursor(0, 0);lcd.print("k= ");lcd.print(k);
}}}}
while (k==1);
//********************** END OF OUTWARD LOOP ************************************************ END OF OUTWARD LOOP ***********************
do
{if (nss.available() > 0 ) {
char c = nss.read(); // read incoming character from GPS
// check if the character completes a valid GPS sentence
if (gps.decode(c)) {
// check if GPS positioning was active
if (gps.gprmc_status() == 'A') {
{d = gps.gprmc_course_to(hlat,hlon) - gps.gprmc_course();} // steer for home
if (ledn==0) {ledn=1; digitalWrite(8,LOW);delay(100);} //navigation LED
if (d < 0) { d += 360; } //convert d to + - 180 degrees
if (d > 180) { d -= 360; }
//Serial.print("d= ");Serial.println(d);
if (d>-1 && d<1) {steerservo.write(90+soff);} // + is turn right 90 degrees is straight ahead
if (d>=2) {
steerservo.write(85+soff);}
if (d>5) {
steerservo.write(80+soff);}
if (d>10) {
steerservo.write(65+soff);}
if (d>20) {
steerservo.write(40+soff);}
if (ledn==1) {ledn=0; digitalWrite(8,HIGH);}
if (d<=-2) {
steerservo.write(95+soff);}
if (d<-5) {
steerservo.write(100+soff);}
if (d<-10) {
steerservo.write(115+soff);}
if (d<-20) {
steerservo.write(140+soff);}
}
speedservo.write(150); // full power on all of homeward leg
int ol=digitalRead(6);
if (ol==1) {speedservo.write(90);
delay(2000);speedservo.write(10);delay(3000);speedservo.write(90);delay(2000);speedservo.write(150); digitalWrite(8,HIGH);
delay(5000);
for (int i=0; i<10; i++)
{digitalWrite(8,LOW); // led on for morse character A for Amp overload (dot dash)
delay (100);
digitalWrite(8,HIGH); //end of dot
delay(100); // led off inter element gap
digitalWrite(8,LOW); delay(300);// led on for dash
lcd.setCursor(0, 0);lcd.print(" ");lcd.setCursor(1, 1);lcd.print(" "); // clear the screen
digitalWrite(8,HIGH);delay(500);}lcd.setCursor(0, 0);lcd.print("Current overload");}
ol=0;
if (lvr !=1) { // LVR IS NOT = TO 1
int lv =digitalRead(11); // if voltage low flash LEDs, just flash the leds to indicate (nothing else can be done)
if (lv==1) {lcd.setCursor(1, 1);lcd.print("Low V homeward "); lvr=1;
for(int i=0; i<10; i++)
{digitalWrite(8,LOW);delay(100); // first dot of character V
digitalWrite(8,HIGH);delay(100); //inter
digitalWrite(8,LOW);delay(100); // 2nd dot
digitalWrite(8,HIGH);delay(100); //inter
digitalWrite(8,LOW);delay(100); // 3rd dot
digitalWrite(8,HIGH);delay(100); // inter
digitalWrite(8,LOW);delay(300); // dash
digitalWrite(8,HIGH);delay(600);}}} // character end
if (gps.gprmc_distance_to(hlat,hlon,MTR) <5)
{k=3;} //Serial.println("at the bottom of home loop");Serial.print("k = "); Serial.println(k);
}
}
}
while (k==2);
//************************************** END OF HOME LOOP ******************************************* END OF HOME LOOP ******************
if (k==3);
{speedservo.write(90); // *****stop motor
for(int e=0; e<10000; e++)
{digitalWrite(8,LOW);
delay (500);
digitalWrite(8,HIGH);
delay(5000);}}} // delay of about 13 hours
