Autopilot for Sailing Boats (Automatic Steering System)

/*
  This sketch act as Autopilot for small sailing boats, by Marco Zonca, 2019
  Arduino UNO as CPU, Arduino Nano as watchdog, GPS BT-220 nmea, stepper motor + controller, rf433Mhz RC, 6 buttons, buzzer, i2c display,
  2 leds, i2c 24c04 eeprom, Mux 4051 for sensors, lipo 2s 7.4v 2600mA, 7805 voltage regulator, Thermistor;
*/

#include <LiquidCrystal_I2C.h>
#include <NewTone.h>
#include <Stepper.h>
#include <Wire.h>

String inputString = "";
String nm_time = "00:00:00";
String nm_validity = "V";
String nm_latitude = "ddmm.mmmm'N";
String nm_longitude = "dddmm.mmmm'E";
String nm_knots = "0.0kn";
float nmf_knots = 0.0;
String nm_truecourse = "360";
float nmf_truecourse = 360;
String nm_date = "dd/mm/yyyy";
String nm_routetofollow ="000";
float nmf_routetofollow = 0;
unsigned long previousStearingMillis = 0;
unsigned long currentStearingMillis = 0;
unsigned long prevCheckSensorsMillis = 0;
unsigned long currCheckSensorsMillis = 0;
int CheckSensorsInterval = 10000;

bool stringComplete = false;
bool isfirstfix = true;
bool ispause = true;
bool isStearing = false;
bool isSetup = false;

int s=0;
int y=0;
int z=0;
int d=0;
int rfRemoteControlValue = 0;
int HWButtonValue = 0;
int SetupParameter = 0;
float calcmove = 0;
float cm = 0;
float Stearing = 0;
float prevStearing = 0;
float t = 0;
int EEdisk = 0x50;
int EEid1 = 0x29;
int EEid2 = 0x00;
unsigned int EEaddress = 0;
unsigned int EEbytes = 12;
byte EEdata[12];
byte EEbytedata;
int EEerr = 0;
float SensorVBatt=0;
float SensorVRes=0;
float SensorTemp=0;
float SensormAmp=0;

// following parameters are the defaults but are read/write in eeprom
// eeprom is initialized if at addresses 0 and 1 the content is different       addres  len   type    notes
// 0-255 bytes at 0x50 EEdisk, 256-512 bytes at 0x51 (not used)                ---------------------------------------------------------------
//                                                                              0       1B    byte    01001001 (0x29 as autopilot project id1)
//                                                                              1       1B    byte    00000000 (0x00       "          "   id2)
int StearingInterval = 2000;  // millis between try and back                    2       2B    int     StearingInterval  1000-5000 steps 100
int StearingMinToMove = 2;  // compass_degrees                                  4       2B    int     StearingMinToMove    0-20   steps   1
int StearingMaxMove = 40;  // compass_degrees                                   6       2B    int     StearingMaxMove     10-45   steps   1
float StearingCoeffMove = 1.5;  // used as (compass_degrees * coeff)            8       4B    float   StearingCoeffMove  0.1-4    steps 0.1
//                                                                              12      free
//
byte bStearingInterval[sizeof(int)];
byte bStearingMinToMove[sizeof(int)];
byte bStearingMaxMove[sizeof(int)];
byte bStearingCoeffMove[sizeof(float)];
int prev_StearingInterval=0;
int prev_StearingMinToMove=0;
int prev_StearingMaxMove=0;
float prev_StearingCoeffMove=0;

const int ledpausePin = 2;
const int watchDogPin = 3;
const int MuxSelBit0Pin = 7;  // 00=Vin 01=Vbatt 10=Temp
const int MuxSelBit1Pin = 6;  // 
const int motorsABenablePin = 13;
const int MuxIOPin = 14;
const int ButtonsPin = 15;
const int rfRemoteControlPin = 16;
const int speakerPin = 17;
const int RCleftbutton = 201;
const int RCrightbutton = 202;
const int RCleft10button = 203;
const int RCright10button = 204;
const int HWleftbutton = 101;
const int HWrightbutton = 102;
const int HWpausebutton = 103;
const int HWsetupbutton = 104;
const int HWleft10button = 105;
const int HWright10button = 106;
const int motorStepsPerRevolution = 200;  // 200 for model 23LM, 54 steps = 1/4 of revolution

LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
Stepper motor(motorStepsPerRevolution, 9, 10, 11, 12);

void setup() {
  Serial.begin(4800);
  lcd.begin(16,2);
  Wire.begin();
  motor.setSpeed(60);
  inputString.reserve(200);
  pinMode(motorsABenablePin, OUTPUT);
  pinMode(MuxSelBit0Pin, OUTPUT);
  pinMode(MuxSelBit1Pin, OUTPUT);
  digitalWrite(motorsABenablePin, LOW);
  digitalWrite(MuxSelBit0Pin, LOW);
  digitalWrite(MuxSelBit1Pin, LOW);
  pinMode(ledpausePin, OUTPUT);
  pinMode(watchDogPin, OUTPUT);
  digitalWrite(ledpausePin, LOW);
  digitalWrite(watchDogPin, LOW);
  
  // read+check EEPROM (formatting if new (or not identified))
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Memory check...");
  lcd.setCursor(0,1);
  for (s = 0; s < EEbytes; s ++) {
    EEaddress = s;
    EEbytedata = readEEPROM (EEdisk, EEaddress);
    EEdata[s] = EEbytedata;
  }
  if (EEerr) {
    lcd.print("E=");
    lcd.print(EEerr);
    delay(1000);
  }
  if ((EEdata[0] != EEid1) || (EEdata[1] != EEid2)) {
    lcd.print(" init! ");
    goupdateEEPROM();
    if (EEerr) {
      lcd.print("E=");
      lcd.print(EEerr);
      delay(1000);
    }
  }
  memcpy(bStearingInterval, EEdata+2, sizeof(int));
  memcpy(bStearingMinToMove, EEdata+4, sizeof(int));
  memcpy(bStearingMaxMove, EEdata+6, sizeof(int));
  memcpy(bStearingCoeffMove, EEdata+8, sizeof(float));
  StearingInterval = *((int*) bStearingInterval);
  StearingMinToMove = *((int*) bStearingMinToMove);
  StearingMaxMove = *((int*) bStearingMaxMove);
  StearingCoeffMove = *((float*) bStearingCoeffMove);
  prev_StearingInterval = StearingInterval;
  prev_StearingMinToMove = StearingMinToMove;
  prev_StearingMaxMove = StearingMaxMove;
  prev_StearingCoeffMove = StearingCoeffMove;
  lcd.print(" OK");
  delay(1000);
  lcd.clear();
  lcd.print("GPS reading...");
  delay(1000);
}

void loop() {
  // CHECK SENSORS -----------------
  currCheckSensorsMillis = millis();
  if (currCheckSensorsMillis - prevCheckSensorsMillis >= CheckSensorsInterval) {
    readMuxSensors();
    if ((SensorVBatt <= 7.0) || (SensorTemp >= 50)) {
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Alarm sensors! ");
      lcd.setCursor(1,1);
      lcd.print("V=");
      lcd.print(SensorVBatt);
      lcd.print("  ");
      lcd.print(int(SensorTemp));
      lcd.write(0xDF);
      lcd.print("C");
      NewTone (speakerPin,10);
      delay(1000);
      noNewTone();
    }
    prevCheckSensorsMillis = currCheckSensorsMillis;
  }
  // STEARING CONTROL ----------------
  currentStearingMillis = millis();
  if (currentStearingMillis - previousStearingMillis >= StearingInterval) {
    if (isStearing == false && ispause == false) {  // go try (move stearing)
      calcmove = nmf_routetofollow - nmf_truecourse;
      if (calcmove < (-180)) {
        calcmove = calcmove + 360;
      } else {
        if (calcmove > (+180)) {
          calcmove = calcmove - 360;
        }
      }
      if (abs(calcmove) >= StearingMinToMove) {
        if (abs(calcmove) >= StearingMaxMove) {
          if (calcmove < 0) {
              cm = (StearingMaxMove * -1);
              calcmove = cm;
            } else {
              cm = (StearingMaxMove * 1);
              calcmove = cm;
          }
        }
        Stearing = (calcmove * StearingCoeffMove);
        gomotor(int((Stearing * 216) / 360));  // 54 steps = 1/4 of revolution
        prevStearing = Stearing;
        isStearing = true;
      }
    } else {  // go back (move stearing to "zero" position)
      if (isStearing == true) {
        Stearing = (prevStearing * -1);
        gomotor(int((Stearing * 216) / 360));  // 54 steps = 1/4 of revolution
        Stearing = 0;
        prevStearing = 0;
        isStearing = false;
      }
    }
    previousStearingMillis = currentStearingMillis;
  }
  // RC RF BUTTONS ------------------
  rfRemoteControlValue = checkRfRC();
  if (rfRemoteControlValue) {
    switch (rfRemoteControlValue) {
      case RCleftbutton: // Left RC button
        goleft();
        break;
      case RCrightbutton: // Right RC button
        goright();
        break;
      case RCleft10button: // Left-10 RC button
        goleft10();
        break;
      case RCright10button: // Right+10 RC button
        goright10();
        break;
    }  
  }  
  // BUTTONS ------------------------
  HWButtonValue = checkHWButtons();
  if (HWButtonValue) {
    switch (HWButtonValue) {
      case HWleftbutton: // Left(-1) HW button
        if (isSetup == false) {
            goleft();
          } else {
            setupMinus();
        }
        break;
      case HWrightbutton: // Right(+1) HW button
        if (isSetup == false) {
            goright();
          } else {
            setupPlus();
        }
        break;
      case HWpausebutton: // Pause HW button
        gopause();
        break;
      case HWsetupbutton: // Setup HW button
        gosetup();
        break;
      case HWleft10button: // Left(-10) HW button
        goleft10();
        break;
      case HWright10button: // Right(+10) HW button
        goright10();
        break;
    }  
  }
  // GPS NMEA ------------------
  if (stringComplete == true) {  // received nmea sentence by serial port RX
    bool ret;
    ret = nmeaExtractData();
    inputString = "";
    stringComplete = false;
    if (ret == true) {
      RefreshDisplay();
    }
  }
  // WATCHDOG FEEDING ----------------
  if (digitalRead(watchDogPin) == LOW) {
    digitalWrite(watchDogPin, HIGH);
  } else {
    digitalWrite(watchDogPin, LOW);
  }
}

// read sensors on multiplexer
void readMuxSensors() {
  float Vo = 0;
  float n = 0;
  float n1 = 0;
  float v1ad = 0;
  float v2ad = 0;
  float corr = 0;
  float R1 = 10000;
  float logR2 = 0;
  float R2 = 0;
  float T = 0;
  float c1 = 1.009249522e-03;
  float c2 = 2.378405444e-04;
  float c3 = 2.019202697e-07;
  digitalWrite(MuxSelBit0Pin, LOW);  // 00=Vbatt
  digitalWrite(MuxSelBit1Pin, LOW);
  n = analogRead(MuxIOPin);
  v1ad=n;
  n1=(((10.00 * n) / 1023.00));
  SensorVBatt=(n1 + ((n1 * 0.0) /100));  // arbitrary correction (not active = 0.0%)
  digitalWrite(MuxSelBit0Pin, LOW);  // 01=Vres
  digitalWrite(MuxSelBit1Pin, HIGH);
  n = analogRead(MuxIOPin);
  v2ad=n;
  n1=(((10.00 * n) / 1023.00));
  SensorVRes=(n1 + ((n1 * 0.0) /100));  // arbitrary correction (not active = 0.0%)
  digitalWrite(MuxSelBit0Pin, HIGH);  // 10=NTC Temp
  digitalWrite(MuxSelBit1Pin, LOW);
  Vo = analogRead(MuxIOPin);
  R2 = R1 * (1023.0 / Vo - 1.0);
  logR2 = log(R2);
  T = (1.0 / (c1 + c2 * logR2 + c3 * logR2 * logR2 * logR2));
  SensorTemp = T - 273.15;  // Celsius
  n = (v1ad - v2ad);
  n1 =  (n / 0.22) * 1000.00;
  SensormAmp = (((10.00 * n1) / 1023.00));
}

// extract data from nmea inputString
bool nmeaExtractData() {
  bool ret = false;  //true if nmea sentence = $GNRMC and valid CHKSUM
  if ((inputString.substring(0,6) == "$GNRMC") && (inputString.substring(inputString.length()-4,inputString.length()-2) == nmea0183_checksum(inputString))) {
    y=0;
    for (s = 1; s < 11; s ++) { 
      y=inputString.indexOf(",",y);
      switch (s) {
      case 1: //time
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nm_time=inputString.substring(y+1,y+2+1)+":"+inputString.substring(y+1+2,y+4+1)+":"+inputString.substring(y+1+4,y+6+1);
        }
        y=z;
        break;
      case 2: //validity
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nm_validity=inputString.substring(y+1,y+1+1);
        }
        y=z;
        break;
      case 3: //latitude
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nm_latitude=inputString.substring(y+1,y+2+1)+""+inputString.substring(y+1+2,y+10+1)+"'";
        }
        y=z;
        break;
      case 4: //north/south
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nm_latitude=nm_latitude + inputString.substring(y+1,y+1+1);
        }
        y=z;
        break;
      case 5: //longitude
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nm_longitude=inputString.substring(y+1,y+3+1)+""+inputString.substring(y+1+3,y+11+1)+"'";
        }
        y=z;
        break;
      case 6: //east/west
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nm_longitude=nm_longitude + inputString.substring(y+1,y+1+1);
        }
        y=z;
        break;
      case 7:  //speed knots
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nmf_knots=inputString.substring(y+1,z).toFloat();
          t=roundOneDec(nmf_knots);
          nm_knots=String(t,1)+"kn";
        }
        y=z;
        break;
      case 8: //true course
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nmf_truecourse=inputString.substring(y+1,z).toFloat();
          d=nmf_truecourse;
          nm_truecourse=d;
        }
        y=z;
        break;
      case 9: //date
        z=inputString.indexOf(",",y+1);
        if (z>(y+1)) {
          nm_date=inputString.substring(y+1,y+2+1)+"/"+inputString.substring(y+1+2,y+4+1)+"/20"+inputString.substring(y+1+4,y+6+1);
        }
        y=z;
        break;
      case 10:
        // statements
        break;
      default:
        // statements
        break;
      }
    }
    if ((isfirstfix == true) || (ispause == true)) {
      nm_routetofollow=nm_truecourse;
      nmf_routetofollow=nmf_truecourse;
      isfirstfix=false;
    }
    ret=true;
  }
  return ret;
}

// increase(+) parameter value during setup
void setupPlus() {
  switch (SetupParameter) {
    case 2: //interval
      StearingInterval = (StearingInterval + 100);
      if (StearingInterval > 5000) {
        StearingInterval = 5000;
      }
      break;
    case 3: //min. to move
      StearingMinToMove = (StearingMinToMove + 1);
      if (StearingMinToMove > 20) {
        StearingMinToMove = 20;
      }
      break;
    case 4: //max. move
      StearingMaxMove = (StearingMaxMove + 1);
      if (StearingMaxMove > 45) {
        StearingMaxMove = 45;
      }
      break;
    case 5: //coefficient
      StearingCoeffMove = (StearingCoeffMove + 0.1);
      if (StearingCoeffMove > 4) {
        StearingCoeffMove = 4;
      }
      break;
  }
  delay(200);
  RefreshDisplay();
}

// decrease(-) parameter value during setup
void setupMinus() {
  switch (SetupParameter) {
    case 2: //interval
      StearingInterval = (StearingInterval - 100);
      if (StearingInterval < 1000) {
        StearingInterval = 1000;
      }
      break;
    case 3: //min. to move
      StearingMinToMove = (StearingMinToMove - 1);
      if (StearingMinToMove < 0) {
        StearingMinToMove = 0;
      }
      break;
    case 4: //max. move
      StearingMaxMove = (StearingMaxMove - 1);
      if (StearingMaxMove < 10) {
        StearingMaxMove = 10;
      }
      break;
    case 5: //coefficient
      StearingCoeffMove = (StearingCoeffMove - 0.1);
      if (StearingCoeffMove < 0.1) {
        StearingCoeffMove = 0.1;
      }
      break;
  }
  delay(200);
  RefreshDisplay();
}

// motor control (+)=forward (-)=backwards
void gomotor(int stepsToMove) {
  digitalWrite(motorsABenablePin, HIGH);
  motor.step(stepsToMove);
  digitalWrite(motorsABenablePin, LOW);
}

// refresh data on display
void RefreshDisplay() {
  if (isSetup == false) {  //---------normal
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("R"+nm_routetofollow);
    lcd.write(0xDF);
    lcd.print(" H"+nm_truecourse);
    lcd.write(0xDF);
    if (ispause == true) {
      lcd.print(" STOP");    
    } else {
      if (Stearing > 0) {
        lcd.print(" +");
      }
      if (Stearing == 0) {
        lcd.print("  ");
      }
      if (Stearing < 0) {
        lcd.print(" ");
      }
      lcd.print(int(Stearing));
    }
    lcd.setCursor(0,1);
    lcd.print(nm_time+" "+nm_knots);
  }
  if (isSetup == true) {  //-----------setup
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("setup: ");
    switch (SetupParameter) {
      case 1: //display sensors
        readMuxSensors(); 
        lcd.print("V=");
        lcd.print(SensorVBatt);
        lcd.setCursor(1,1);
        lcd.print("mA=");
        lcd.print(int(SensormAmp));
        lcd.print("  ");
        lcd.print(int(SensorTemp));
        lcd.write(0xDF);
        lcd.print("C");
        break;
      case 2: //interval
        lcd.print("interval");
        lcd.setCursor(7,1);
        lcd.print(StearingInterval);
        lcd.print(" mSec");
        break;
      case 3: //min. to move
        lcd.print("minimum");
        lcd.setCursor(7,1);
        lcd.print(StearingMinToMove);
        lcd.write(0xDF);
        break;
      case 4: //max. move
        lcd.print("max");
        lcd.setCursor(7,1);
        lcd.print(StearingMaxMove);
        lcd.write(0xDF);
       break;
      case 5: //coefficient
        lcd.print("coeffic.");
        lcd.setCursor(7,1);
        lcd.print(StearingCoeffMove);
        lcd.print(" x ");
        lcd.write(0xDF);
        break;
    }
  }
}
/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
  while (Serial.available()) {
    char inChar = (char)Serial.read();
    inputString += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it
    if (inChar == '\n') {
      stringComplete = true;
      }
    }
  }

//calculate checksum of nmea sentence
String nmea0183_checksum(String nmea_data) {
    int crc = 0;
    String chSumString = "";
    int i;
    // ignore the first $ sign, checksum in sentence
    for (i = 1; i < (nmea_data.length()-5); i ++) { // remove the - 5 if no "*" + cksum + cr + lf are present
        crc ^= nmea_data[i];
    }
    chSumString = String(crc,HEX);
    if (chSumString.length()==1) {
      chSumString="0"+chSumString.substring(0,1);
    }
    chSumString.toUpperCase();
    return chSumString;
}

//check RC which button is pressed
int checkRfRC() {
  int n = 0;
  int res = 0;
  n = analogRead(rfRemoteControlPin);
  if ((n>350) and (n<460)) { // button A
    res = RCleftbutton;
  }
  if ((n> 90) and (n<190)) { // button B
    res = RCrightbutton;
  }
  if ((n>540) and (n<640)) { // button C
    res = RCleft10button;
  }
  if ((n>225) and (n<325)) { // button D
    res = RCright10button;
  }
  return res;    
}

//check HW which button is pressed
int checkHWButtons() {
  int n = 0;
  int res = 0;
  n = analogRead(ButtonsPin);  
  //Serial.println(n); 
  if ((n>465) and (n<565)) { // button left
    res = HWleftbutton;
  }
  if ((n>290) and (n<390)) { // button right
    res = HWrightbutton;
  }
  if ((n>130) and (n<220)) { // button pause
    res = HWpausebutton;
  }
  if ((n>625) and (n<725)) { // button setup
    res = HWsetupbutton;
  }
  if ((n>975) and (n<1075)) { // button left-10
    res = HWleft10button;
  }
  if ((n>800) and (n<900)) { // button right+10
    res = HWright10button;
  }
  return res;    
}

void gosetup() {  // setup button
  if (isSetup == false) {
      SetupParameter = 1;
      isSetup = true;
    } else {
      if (SetupParameter < 5) {
          SetupParameter ++;
        } else {
          if (prev_StearingInterval != StearingInterval || prev_StearingMinToMove != StearingMinToMove || prev_StearingMaxMove != StearingMaxMove || prev_StearingCoeffMove != StearingCoeffMove) {
            lcd.clear();
            lcd.setCursor(0,0);
            lcd.print("updating... ");
            delay(1000);
            goupdateEEPROM();
            if (EEerr) {
              lcd.print("E=");
              lcd.print(EEerr);
              delay(1000);
            }
            prev_StearingInterval = StearingInterval;
            prev_StearingMinToMove = StearingMinToMove;
            prev_StearingMaxMove = StearingMaxMove;
            prev_StearingCoeffMove = StearingCoeffMove;
          }
          isSetup = false;
      }
  }
  NewTone (speakerPin,2000);
  delay(200);
  noNewTone();
  RefreshDisplay();
}

void goupdateEEPROM() {
  EEaddress = 0;  //id1
  EEdata[0] = EEid1;
  EEbytedata = EEid1;
  writeEEPROM (EEdisk, EEaddress, EEbytedata);
  EEaddress = 1;  //id2
  EEdata[1] = EEid2;
  EEbytedata = EEid2;
  writeEEPROM (EEdisk, EEaddress, EEbytedata);
  memcpy(bStearingInterval, &StearingInterval, sizeof(int));
  memcpy(bStearingMinToMove, &StearingMinToMove, sizeof(int));
  memcpy(bStearingMaxMove, &StearingMaxMove, sizeof(int));
  memcpy(bStearingCoeffMove, &StearingCoeffMove, sizeof(float));
  memcpy(EEdata+2,bStearingInterval,sizeof(int));
  memcpy(EEdata+4,bStearingMinToMove,sizeof(int));
  memcpy(EEdata+6,bStearingMaxMove,sizeof(int));
  memcpy(EEdata+8,bStearingCoeffMove,sizeof(float));
  for (s = 2; s < EEbytes; s ++) {
    EEaddress = s;  //data
    EEbytedata = EEdata[s];
    writeEEPROM (EEdisk, EEaddress, EEbytedata);
  }
}

void goleft() {  // left button/RC
  if (ispause == false) {
    nmf_routetofollow --;
    if (nmf_routetofollow < 1) {
      nmf_routetofollow = 360;
    }
    d=nmf_routetofollow;
    nmf_routetofollow=d;
    nm_routetofollow=d;
    NewTone (speakerPin,400);
    delay(200);
    noNewTone();
  } else {
    NewTone (speakerPin,1000);
    delay(50);
    noNewTone();
  }
  RefreshDisplay();
}

void goleft10() {  // left 10x button/RC
  if (ispause == false) {
    for (s = 1; s < 11; s ++) {
      nmf_routetofollow --;
      if (nmf_routetofollow < 1) {
        nmf_routetofollow = 360;
      }
    }
    d=nmf_routetofollow;
    nmf_routetofollow=d;
    nm_routetofollow=d;
    NewTone (speakerPin,400);
    delay(200);
    noNewTone();
  } else {
    NewTone (speakerPin,1000);
    delay(50);
    noNewTone();
  }
  RefreshDisplay();
}

void goright() {  // right button/RC
  if (ispause == false) {
    nmf_routetofollow ++;
    if (nmf_routetofollow > 360) {
      nmf_routetofollow = 1;
    }
    d=nmf_routetofollow;
    nmf_routetofollow=d;
    nm_routetofollow=d;
    NewTone (speakerPin,800);
    delay(200);
    noNewTone();
  } else {
    NewTone (speakerPin,1000);
    delay(50);
    noNewTone();
  }
  RefreshDisplay();
}

void goright10() {  // right 10x button/RC
  if (ispause == false) {
    for (s = 1; s < 11; s ++) {
      nmf_routetofollow ++;
      if (nmf_routetofollow > 360) {
        nmf_routetofollow = 1;
      }
    }
    d=nmf_routetofollow;
    nmf_routetofollow=d;
    nm_routetofollow=d;
    NewTone (speakerPin,800);
    delay(200);
    noNewTone();
  } else {
    NewTone (speakerPin,1000);
    delay(50);
    noNewTone();
  }
  RefreshDisplay();
}

void gopause() {  // pause button/RC
  if (ispause == true) {
    ispause=false; 
    digitalWrite(ledpausePin, HIGH);
    NewTone (speakerPin,50);
    delay(200);
    NewTone (speakerPin,200);
    delay(800);
    noNewTone();
  } else {
    ispause=true; 
    digitalWrite(ledpausePin, LOW);
    NewTone (speakerPin,200);
    delay(200);
    NewTone (speakerPin,50);
    delay(800);
    noNewTone();
  }
  RefreshDisplay();
}

// reading eeprom
byte readEEPROM (int diskaddress, unsigned int memaddress) {
  byte rdata = 0x00;
  Wire.beginTransmission (diskaddress);
  Wire.write (memaddress);
  if (Wire.endTransmission () == 0) {
    Wire.requestFrom (diskaddress,1);
    if (Wire.available()) {
        rdata = Wire.read();
      } else {
        EEerr = 1; //"READ no data available"
    }
    } else {
      EEerr = 2; //"READ eTX error"
  }
  Wire.endTransmission (true);
  return rdata;
}

// writing eeprom
void writeEEPROM (int diskaddress, unsigned int memaddress, byte bytedata) {
  Wire.beginTransmission (diskaddress);
  Wire.write (memaddress);
  Wire.write (bytedata);
  if (Wire.endTransmission () != 0) {
    EEerr = 3; //"WRITING eTX error"
  }
  Wire.endTransmission (true);
  delay(5); 
}

// round zero decimal
float roundZeroDec(float f) {
  float y, d;
  y = f*1;
  d = y - (int)y;
  y = (float)(int)(f*1)/1;
  if (d >= 0.5) {
    y += 1;
   } else {
    if (d < -0.5) {
    y -= 1;
  }
  }
  return y;
}

// round one decimal
float roundOneDec(float f) {
  float y, d;
  y = f*10;
  d = y - (int)y;
  y = (float)(int)(f*10)/10;
  if (d >= 0.5) {
    y += 0.1;
   } else {
    if (d < -0.5) {
    y -= 0.1;
  }
  }
  return y;
}

// round two decimal
float roundTwoDec(float f) {
  float y, d;
  y = f*100;
  d = y - (int)y;
  y = (float)(int)(f*100)/100;
  if (d >= 0.5) {
    y += 0.01;
   } else {
    if (d < -0.5) {
    y -= 0.01;
  }
  }
  return y;
}