Autopilot for sailing boats (NEW! - Version 2)

// AUTOPILOT version 2
// by Marco Zonca, 2019-2022
/*  
   This sketch works as Autopilot for small sailing boats
   Arduino Nano as CPU, Arduino Nano as watchdog, GPS BT-220 nmea, stepper motor + controller, rf315Mhz RC, 6 buttons,
   buzzer, i2c display, 3xLEDS, i2c 24c04 eeprom, Mux 4051 for trasducers (sensors), lipo 2s 7.4v 2600mA, 
   7805 voltage regulator, Thermistor NTC, ADC MCP3424, etc.;
*/

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

String inputString = "";
String tzone = "00";
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;
uint8_t ADCaddr = 0x68;
unsigned int EEaddress = 0;
unsigned int EEbytes = 24;  // eeprom nr of bytes to r/w, see also EEdata[ ]
byte EEdata[24];  // put the same as EEbytes
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 = 800;  // millis between try and back                     2       2B    int     StearingInterval   100-5000 step 100
int StearingMinToMove = 4;  // compass_degrees                                  4       2B    int     StearingMinToMove    1-20   step   1
int StearingMaxMove = 90;  // compass_degrees                                   6       2B    int     StearingMaxMove     10-360  step  10
//                                                                              8       2B    int     StearingSpeedOut     1-100  step   1
//                                                                              10      2B    int     StearingSpeedBack    1-100  step   1
//                                                                              12      2B    int     StearingKP           0-400  step  10
//                                                                              14      2B    int     StearingKI           0-400  step  10
//                                                                              16      2B    int     StearingKD           0-400  step  10
//                                                                              18      2B    int     StearingReverse      0-1    step   1
//                                                                              20      2B    int     StearingPWM        100-255  step   1
//                                                                              22      2B    int     StearingTZone      -12-+12  step   1
//                                                                              24      free
int StearingSpeedOut = 40;  // speed move out ("fast") steps/sec
int StearingSpeedBack = 20; // speed move back ("slow") steps/sec
int StearingKP = 150; // PID "P" 1.00 (cents)
int StearingKI = 10; // PID "I" 0.20 (cents)
int StearingKD = 10; // PID "D" 0.20 (cents)
int StearingReverse = 0; // 0=false 1=true (clockwise or not)
int StearingPWM = 245;  // PWM on enable motor pins
int StearingTZone = 0;  // GMT=0 Italy is +1 or +2

byte bStearingInterval[sizeof(int)];
byte bStearingMinToMove[sizeof(int)];
byte bStearingMaxMove[sizeof(int)];
byte bStearingSpeedOut[sizeof(int)];
byte bStearingSpeedBack[sizeof(int)];
byte bStearingKP[sizeof(int)];
byte bStearingKI[sizeof(int)];
byte bStearingKD[sizeof(int)];
byte bStearingReverse[sizeof(int)];
byte bStearingPWM[sizeof(int)];
byte bStearingTZone[sizeof(int)];

int prev_StearingInterval=0;
int prev_StearingMinToMove=0;
int prev_StearingMaxMove=0;
int prev_StearingSpeedOut=0;
int prev_StearingSpeedBack=0;
int prev_StearingKP=0;
int prev_StearingKI=0;
int prev_StearingKD=0;
int prev_StearingReverse=0;
int prev_StearingPWM=0;
int prev_StearingTZone=0;

const int ledpausePin = 2;
const int watchDogPin = 3;
const int MuxSelBit0Pin = 8;  // 000=Vin 001=Vbatt 010=Temp
const int MuxSelBit1Pin = 7;  // 
const int MuxSelBit2Pin = 6;  // 
const int motorsABenablePin = 5;  // PWM
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 = 216;  // 216 steps for model 23LM, 1.8 per step, 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);
MCP342x adc = MCP342x(ADCaddr);
PID_v2 myPID((float)(StearingKP/100.0),(float)(StearingKI/100.0),(float)(StearingKD/100.0), PID::Direct);  // P, I, D, d/r

void setup() {
  Serial.begin(4800);
  lcd.begin(16,2);
  Wire.begin();
  inputString.reserve(200);
  pinMode(motorsABenablePin, OUTPUT);
  pinMode(MuxSelBit0Pin, OUTPUT);
  pinMode(MuxSelBit1Pin, OUTPUT);
  pinMode(MuxSelBit2Pin, OUTPUT);
  digitalWrite(motorsABenablePin, LOW);
  digitalWrite(MuxSelBit0Pin, LOW);
  digitalWrite(MuxSelBit1Pin, LOW);
  digitalWrite(MuxSelBit2Pin, LOW);
  pinMode(ledpausePin, OUTPUT);
  pinMode(watchDogPin, OUTPUT);
  digitalWrite(ledpausePin, LOW);
  digitalWrite(watchDogPin, LOW);

  MCP342x::generalCallReset();
  delay(1); // MC342x needs 300us to settle, wait 1ms
  
  // read+check EEPROM (formatting (initializing) 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(5000);
  }
  if ((EEdata[0] != EEid1) || (EEdata[1] != EEid2)) {
    lcd.print(" init! ");
    goupdateEEPROM();
    if (EEerr) {
      lcd.print("E=");
      lcd.print(EEerr);
      delay(5000);
    }
  }
  memcpy(bStearingInterval, EEdata+2, sizeof(int));
  memcpy(bStearingMinToMove, EEdata+4, sizeof(int));
  memcpy(bStearingMaxMove, EEdata+6, sizeof(int));
  memcpy(bStearingSpeedOut, EEdata+8, sizeof(int));
  memcpy(bStearingSpeedBack, EEdata+10, sizeof(int));
  memcpy(bStearingKP, EEdata+12, sizeof(int));
  memcpy(bStearingKI, EEdata+14, sizeof(int));
  memcpy(bStearingKD, EEdata+16, sizeof(int));
  memcpy(bStearingReverse, EEdata+18, sizeof(int));
  memcpy(bStearingPWM, EEdata+20, sizeof(int));
  memcpy(bStearingTZone, EEdata+22, sizeof(int));

  StearingInterval = *((int*) bStearingInterval);
  StearingMinToMove = *((int*) bStearingMinToMove);
  StearingMaxMove = *((int*) bStearingMaxMove);
  StearingSpeedOut = *((int*) bStearingSpeedOut);
  StearingSpeedBack = *((int*) bStearingSpeedBack);
  StearingKP = *((int*) bStearingKP);
  StearingKI = *((int*) bStearingKI);
  StearingKD = *((int*) bStearingKD);
  StearingReverse = *((int*) bStearingReverse);
  StearingPWM = *((int*) bStearingPWM);
  StearingTZone = *((int*) bStearingTZone);
  
  prev_StearingInterval = StearingInterval;
  prev_StearingMinToMove = StearingMinToMove;
  prev_StearingMaxMove = StearingMaxMove;
  prev_StearingSpeedOut = StearingSpeedOut;
  prev_StearingSpeedBack = StearingSpeedBack;
  prev_StearingKP = StearingKP;
  prev_StearingKI = StearingKI;
  prev_StearingKD = StearingKD;
  prev_StearingReverse = StearingReverse;
  prev_StearingPWM = StearingPWM;
  prev_StearingTZone = StearingTZone;

  myPID.SetOutputLimits((StearingMaxMove*(-1)), StearingMaxMove);
  myPID.Start(0, 0, 0); // input, output, setpoint (direction, correction, target)
  myPID.SetSampleTime(StearingInterval);
  
  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 <= 6.8) || (SensorTemp >= 60)) {
      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) {
      myPID.SetOutputLimits((StearingMaxMove*(-1)), StearingMaxMove);
      myPID.SetSampleTime(StearingInterval);
      calcmove = nmf_routetofollow - nmf_truecourse;
      if (calcmove < (-180)) {
        calcmove = calcmove + 360;
      } else {
        if (calcmove > (+180)) {
          calcmove = calcmove - 360;
        }
      }
      if (StearingReverse==1) {
        calcmove = (calcmove * -1);
      }
      if (abs(calcmove) >= StearingMinToMove) {  // go try (move stearing)
        myPID.SetTunings((float)(StearingKP/100.0),(float)(StearingKI/100.0),(float)(StearingKD/100.0));
        Stearing = myPID.Run(calcmove);  // calc PID correction
        motor.setSpeed(StearingSpeedOut);
        gomotor(int((Stearing * 216) / 360));  // 54 steps = 1/4 of revolution (90), 216 = 1 revolution (360)
        prevStearing = Stearing;
        isStearing = true;
      }
    } else {  // go back (move stearing to "zero" position)
      if (isStearing == true) {
        Stearing = (prevStearing * -1);
        motor.setSpeed(StearingSpeedBack);
        gomotor(int((Stearing * 216) / 360));  // 54 steps = 1/4 of revolution (90), 216 = 1 revolution (360)
        Stearing = 0;
        prevStearing = 0;
        isStearing = false;
      }
    }
    previousStearingMillis = currentStearingMillis;
  }
  // RC RF BUTTONS ------------------
  rfRemoteControlValue = checkRfRC();
  if (rfRemoteControlValue) {
    switch (rfRemoteControlValue) {
      case RCleftbutton: // Left -1 RC button
        goleft();
        break;
      case RCrightbutton: // Right +1 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, trasducers on multiplexer, ADC 
void readMuxSensors() {
  uint8_t err = 0;
  int x = 0;
  long ADCval = 0;
  float Fmem = 0;
  float Vo = 0;
  float n = 0;
  float n1 = 0;
  float n2 = 0;
  float corr = 0;
  float logR2 = 0;
  float R2 = 0;
  float T = 0;
  float R1 = 100000;  // 100k resistor in NTC voltage divider
  float c1 = 6.66082410500E-004;  // Steinhart-Hart coeff. 1 for NTC
  float c2 = 2.23928204100E-004;  // Steinhart-Hart coeff. 2 for NTC
  float c3 = 7.19951882000E-008;  // Steinhart-Hart coeff. 3 for NTC
  
  digitalWrite(MuxSelBit0Pin, LOW);  // 000=Vbatt
  digitalWrite(MuxSelBit1Pin, LOW);
  digitalWrite(MuxSelBit2Pin, LOW);
  Fmem=0;
  for (x=1;x<=33;x++) {
    n = analogRead(MuxIOPin);
    n1=(((10.00 * n) / 1023.00));
    Fmem=Fmem+n1;
  }
  n1=(Fmem/(x-1));
  n2=(n1 + ((n1 * 1.15) /100));  // arbitrary correction (not active = 0.0%)
  SensorVBatt=roundTwoDec(n2);
  
  digitalWrite(MuxSelBit0Pin, HIGH);  // 001=Vres
  digitalWrite(MuxSelBit1Pin, LOW);
  digitalWrite(MuxSelBit2Pin, LOW);
  Fmem=0;
  for (x=1;x<=33;x++) {
    n = analogRead(MuxIOPin);
    n1=(((10.00 * n) / 1023.00));
    Fmem=Fmem+n1;
  }
  n1=(Fmem/(x-1));
  n2=(n1 + ((n1 * 1.15) /100));  // arbitrary correction (not active = 0.0%)
  SensorVRes=roundTwoDec(n2);
  
  digitalWrite(MuxSelBit0Pin, LOW);  // 010=NTC Temp
  digitalWrite(MuxSelBit1Pin, HIGH);
  digitalWrite(MuxSelBit2Pin, LOW);
  Fmem=0;
  for (x=1;x<=33;x++) {
    Vo = analogRead(MuxIOPin);
    R2 = R1 * (1023.0 / Vo - 1.0);  // Steinhart-Hart Temperature Calc.
    logR2 = log(R2);
    T = (1.0 / (c1 + c2 * logR2 + c3 * logR2 * logR2 * logR2));
    n1 = T - 273.15;  // Celsius
    Fmem=Fmem+n1;
  }
  n2=(Fmem/(x-1));
  SensorTemp=roundZeroDec(n2);

  MCP342x::Config status;  // differential ADC
  err = adc.convertAndRead(MCP342x::channel1,MCP342x::oneShot,MCP342x::resolution12,MCP342x::gain2,1000000,ADCval,status);
  n=(ADCval / 1.0);  // (ADCgain=2 / Vdivider=2) = 1.0
  n1=(n + ((n * 0.0) /100));  // arbitrary correction (not active = 0.0%)
  n2=(n1/0.22);  // n1=VADC, 0.22ohm = shunt resistor, mA
  SensormAmp=roundZeroDec(n2);
}

// 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)) {
          t=inputString.substring(y+1,y+2+1).toFloat();
          t=t+StearingTZone;
          if (t>23) {
            t=t-24;
          }
          if (t<0) {
            t=t+24;
          }
          tzone="0"+String(t,0);
          nm_time=tzone.substring(tzone.length()-2)+":"+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 + 10);
      if (StearingMaxMove > 360) {
        StearingMaxMove = 360;
      }
      break;
    case 5: //speed out
      StearingSpeedOut = (StearingSpeedOut + 1);
      if (StearingSpeedOut > 100) {
        StearingSpeedOut = 100;
      }
      break;
    case 6: //speed back
      StearingSpeedBack = (StearingSpeedBack + 1);
      if (StearingSpeedBack > 100) {
        StearingSpeedBack = 100;
      }
      break;
    case 7: //KP
      StearingKP = (StearingKP + 10);
      if (StearingKP > 400) {
        StearingKP = 400;
      }
      break;
    case 8: //KI
      StearingKI = (StearingKI + 10);
      if (StearingKI > 400) {
        StearingKI = 400;
      }
      break;
    case 9: //KD
      StearingKD = (StearingKD + 10);
      if (StearingKD > 400) {
        StearingKD = 400;
      }
      break;
    case 10: //reverse
      StearingReverse = (StearingReverse + 1);
      if (StearingReverse > 1) {
        StearingReverse = 1;
      }
      break;
    case 11: //PWM
      StearingPWM = (StearingPWM + 1);
      if (StearingPWM > 255) {
        StearingPWM = 255;
      }
      break;
    case 12: //TZone
      StearingTZone = (StearingTZone + 1);
      if (StearingTZone > 12) {
        StearingTZone = 12;
      }
      break;
  }
  delay(400);
  RefreshDisplay();
}

// decrease(-) parameter value during setup
void setupMinus() {
  switch (SetupParameter) {
    case 2: //interval
      StearingInterval = (StearingInterval - 100);
      if (StearingInterval < 100) {
        StearingInterval = 100;
      }
      break;
    case 3: //min. to move
      StearingMinToMove = (StearingMinToMove - 1);
      if (StearingMinToMove < 0) {
        StearingMinToMove = 0;
      }
      break;
    case 4: //max. move
      StearingMaxMove = (StearingMaxMove - 10);
      if (StearingMaxMove < 10) {
        StearingMaxMove = 10;
      }
      break;
    case 5: //speed out
      StearingSpeedOut = (StearingSpeedOut - 1);
      if (StearingSpeedOut < 1) {
        StearingSpeedOut = 1;
      }
      break;
    case 6: //speed back
      StearingSpeedBack = (StearingSpeedBack - 1);
      if (StearingSpeedBack < 1) {
        StearingSpeedBack = 1;
      }
      break;
    case 7: //KP
      StearingKP = (StearingKP - 10);
      if (StearingKP < 0) {
        StearingKP = 0;
      }
      break;
    case 8: //KI
      StearingKI = (StearingKI - 10);
      if (StearingKI < 0) {
        StearingKI = 0;
      }
      break;
    case 9: //KD
      StearingKD = (StearingKD - 10);
      if (StearingKD < 0) {
        StearingKD = 0;
      }
      break;
    case 10: //reverse
      StearingReverse = (StearingReverse - 1);
      if (StearingReverse < 0) {
        StearingReverse = 0;
      }
      break;
    case 11: //PWM
      StearingPWM = (StearingPWM - 1);
      if (StearingPWM < 100) {
        StearingPWM = 100;
      }
      break;
    case 12: //TZone
      StearingTZone = (StearingTZone - 1);
      if (StearingTZone < -12) {
        StearingTZone = -12;
      }
      break;
  }
  delay(400);
  RefreshDisplay();
}

// motor control (+)=forward (-)=backwards
void gomotor(int stepsToMove) {
  analogWrite(motorsABenablePin, StearingPWM);  // on (PWM)
  motor.step(stepsToMove);
  analogWrite(motorsABenablePin, 0);  // off
}

// 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) {
      if (nm_validity=="V") {  //not valid data, no sat fix
        lcd.print(" sat?");
       } else {
        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: //speed out
        lcd.print("speed Out");
        lcd.setCursor(7,1);
        lcd.print(StearingSpeedOut);
        break;
      case 6: //speed back
        lcd.print("speed Bk");
        lcd.setCursor(7,1);
        lcd.print(StearingSpeedBack);
        break;
      case 7: //KP
        lcd.print("coeff.P");
        lcd.setCursor(7,1);
        lcd.print((float)StearingKP/100.0);
        break;
      case 8: //KI
        lcd.print("coeff.I");
        lcd.setCursor(7,1);
        lcd.print((float)StearingKI/100.0);
        break;
      case 9: //KD
        lcd.print("coeff.D");
        lcd.setCursor(7,1);
        lcd.print((float)StearingKD/100.0);
        break;
      case 10: //reverse
        lcd.print("clockwise");
        lcd.setCursor(7,1);
        if (StearingReverse==0) {
          lcd.print("Direct");
        } else {
          lcd.print("Reverse");
        }
        break;
      case 11: //PWM
        lcd.print("PWM");
        lcd.setCursor(7,1);
        lcd.print(StearingPWM);
        break;
      case 12: //TZone
        lcd.print("TimeZone");
        lcd.setCursor(7,1);
        if (StearingTZone > 0) lcd.print("+");
        lcd.print(StearingTZone);
        break;
    }
  }
}

void serialEvent() {  // it runs between each time loop(), multiple data may be available
  while (Serial.available()) {
    char inChar = (char)Serial.read();
    inputString += inChar;
    if (inChar == '\n') {  // if NL then an NMEA sentence is complete
      stringComplete = true;
      }
    }
  }

String nmea0183_checksum(String nmea_data) {  //calculate checksum of NMEA sentence
    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);
  //Serial.println(n);
  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>90) and (n<190)) { // button pause 143
    res = HWpausebutton;
  }
  if ((n>220) and (n<320)) { // button right 265
    res = HWrightbutton;
  }
  if ((n>340) and (n<440)) { // button left 388
    res = HWleftbutton;
  }
  if ((n>480) and (n<580)) { // button right+10 532
    res = HWright10button;
  }
  if ((n>670) and (n<770)) { // button setup 724
    res = HWsetupbutton;
  }
  if ((n>960) and (n<1060)) { // button left-10 1023
    res = HWleft10button;
  }
  return res;    
}

void gosetup() {  // setup button
  if (isSetup == false) {
      SetupParameter = 1;
      isSetup = true;
    } else {
      if (SetupParameter < 12) {
          SetupParameter ++;
        } else {
          if (prev_StearingInterval != StearingInterval || prev_StearingMinToMove != StearingMinToMove || prev_StearingMaxMove != StearingMaxMove 
              || prev_StearingSpeedOut != StearingSpeedOut || prev_StearingSpeedBack != StearingSpeedBack || prev_StearingKP != StearingKP
                || prev_StearingKI != StearingKI || prev_StearingKD != StearingKD || prev_StearingReverse != StearingReverse
                  || prev_StearingPWM != StearingPWM || prev_StearingTZone != StearingTZone) {
            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_StearingSpeedOut = StearingSpeedOut;
            prev_StearingSpeedBack = StearingSpeedBack;
            prev_StearingKP = StearingKP;
            prev_StearingKI = StearingKI;
            prev_StearingKD = StearingKD;
            prev_StearingReverse = StearingReverse;
            prev_StearingPWM = StearingPWM;
            prev_StearingTZone = StearingTZone;
          }
          isSetup = false;
      }
  }
  NewTone (speakerPin,2000);
  delay(400);
  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(bStearingSpeedOut, &StearingSpeedOut, sizeof(int));
  memcpy(bStearingSpeedBack, &StearingSpeedBack, sizeof(int));
  memcpy(bStearingKP, &StearingKP, sizeof(int));
  memcpy(bStearingKI, &StearingKI, sizeof(int));
  memcpy(bStearingKD, &StearingKD, sizeof(int));
  memcpy(bStearingReverse, &StearingReverse, sizeof(int));
  memcpy(bStearingPWM, &StearingPWM, sizeof(int));
  memcpy(bStearingTZone, &StearingTZone, sizeof(int));

  memcpy(EEdata+2,bStearingInterval,sizeof(int));
  memcpy(EEdata+4,bStearingMinToMove,sizeof(int));
  memcpy(EEdata+6,bStearingMaxMove,sizeof(int));
  memcpy(EEdata+8,bStearingSpeedOut,sizeof(int));
  memcpy(EEdata+10,bStearingSpeedBack,sizeof(int));
  memcpy(EEdata+12,bStearingKP,sizeof(int));
  memcpy(EEdata+14,bStearingKI,sizeof(int));
  memcpy(EEdata+16,bStearingKD,sizeof(int));
  memcpy(EEdata+18,bStearingReverse,sizeof(int));
  memcpy(EEdata+20,bStearingPWM,sizeof(int));
  memcpy(EEdata+22,bStearingTZone,sizeof(int));

  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 decimals
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;
}