Autopilot for Sailing Boats (Automatic Steering System)

1/*
2  This sketch act as Autopilot for small sailing boats, by Marco
3  Zonca, 2019
4  Arduino UNO as CPU, Arduino Nano as watchdog, GPS BT-220 nmea,
5  stepper motor + controller, rf433Mhz RC, 6 buttons, buzzer, i2c display,
6  2
7  leds, i2c 24c04 eeprom, Mux 4051 for sensors, lipo 2s 7.4v 2600mA, 7805 voltage
8  regulator, Thermistor;
9*/
10
11#include <LiquidCrystal_I2C.h>
12#include <NewTone.h>
13#include
14  <Stepper.h>
15#include <Wire.h>
16
17String inputString = "";
18String nm_time
19  = "00:00:00";
20String nm_validity = "V";
21String nm_latitude = "ddmm.mmmm'N";
22String
23  nm_longitude = "dddmm.mmmm'E";
24String nm_knots = "0.0kn";
25float nmf_knots
26  = 0.0;
27String nm_truecourse = "360";
28float nmf_truecourse = 360;
29String
30  nm_date = "dd/mm/yyyy";
31String nm_routetofollow ="000";
32float nmf_routetofollow
33  = 0;
34unsigned long previousStearingMillis = 0;
35unsigned long currentStearingMillis
36  = 0;
37unsigned long prevCheckSensorsMillis = 0;
38unsigned long currCheckSensorsMillis
39  = 0;
40int CheckSensorsInterval = 10000;
41
42bool stringComplete = false;
43bool
44  isfirstfix = true;
45bool ispause = true;
46bool isStearing = false;
47bool isSetup
48  = false;
49
50int s=0;
51int y=0;
52int z=0;
53int d=0;
54int rfRemoteControlValue
55  = 0;
56int HWButtonValue = 0;
57int SetupParameter = 0;
58float calcmove = 0;
59float
60  cm = 0;
61float Stearing = 0;
62float prevStearing = 0;
63float t = 0;
64int
65  EEdisk = 0x50;
66int EEid1 = 0x29;
67int EEid2 = 0x00;
68unsigned int EEaddress
69  = 0;
70unsigned int EEbytes = 12;
71byte EEdata[12];
72byte EEbytedata;
73int
74  EEerr = 0;
75float SensorVBatt=0;
76float SensorVRes=0;
77float SensorTemp=0;
78float
79  SensormAmp=0;
80
81// following parameters are the defaults but are read/write
82  in eeprom
83// eeprom is initialized if at addresses 0 and 1 the content is different
84       addres  len   type    notes
85// 0-255 bytes at 0x50 EEdisk, 256-512 bytes
86  at 0x51 (not used)                ---------------------------------------------------------------
87//
88                                                                              0
89       1B    byte    01001001 (0x29 as autopilot project id1)
90//                                                                              1
91       1B    byte    00000000 (0x00       "          "   id2)
92int StearingInterval
93  = 2000;  // millis between try and back                    2       2B    int     StearingInterval
94  1000-5000 steps 100
95int StearingMinToMove = 2;  // compass_degrees                                  4
96       2B    int     StearingMinToMove    0-20   steps   1
97int StearingMaxMove
98  = 40;  // compass_degrees                                   6       2B    int     StearingMaxMove
99     10-45   steps   1
100float StearingCoeffMove = 1.5;  // used as (compass_degrees
101  * coeff)            8       4B    float   StearingCoeffMove  0.1-4    steps 0.1
102//
103                                                                              12
104      free
105//
106byte bStearingInterval[sizeof(int)];
107byte bStearingMinToMove[sizeof(int)];
108byte
109  bStearingMaxMove[sizeof(int)];
110byte bStearingCoeffMove[sizeof(float)];
111int
112  prev_StearingInterval=0;
113int prev_StearingMinToMove=0;
114int prev_StearingMaxMove=0;
115float
116  prev_StearingCoeffMove=0;
117
118const int ledpausePin = 2;
119const int watchDogPin
120  = 3;
121const int MuxSelBit0Pin = 7;  // 00=Vin 01=Vbatt 10=Temp
122const int MuxSelBit1Pin
123  = 6;  // 
124const int motorsABenablePin = 13;
125const int MuxIOPin = 14;
126const
127  int ButtonsPin = 15;
128const int rfRemoteControlPin = 16;
129const int speakerPin
130  = 17;
131const int RCleftbutton = 201;
132const int RCrightbutton = 202;
133const
134  int RCleft10button = 203;
135const int RCright10button = 204;
136const int HWleftbutton
137  = 101;
138const int HWrightbutton = 102;
139const int HWpausebutton = 103;
140const
141  int HWsetupbutton = 104;
142const int HWleft10button = 105;
143const int HWright10button
144  = 106;
145const int motorStepsPerRevolution = 200;  // 200 for model 23LM, 54 steps
146  = 1/4 of revolution
147
148LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7, 3,
149  POSITIVE);
150Stepper motor(motorStepsPerRevolution, 9, 10, 11, 12);
151
152void
153  setup() {
154  Serial.begin(4800);
155  lcd.begin(16,2);
156  Wire.begin();
157
158  motor.setSpeed(60);
159  inputString.reserve(200);
160  pinMode(motorsABenablePin,
161  OUTPUT);
162  pinMode(MuxSelBit0Pin, OUTPUT);
163  pinMode(MuxSelBit1Pin, OUTPUT);
164
165  digitalWrite(motorsABenablePin, LOW);
166  digitalWrite(MuxSelBit0Pin, LOW);
167
168  digitalWrite(MuxSelBit1Pin, LOW);
169  pinMode(ledpausePin, OUTPUT);
170  pinMode(watchDogPin,
171  OUTPUT);
172  digitalWrite(ledpausePin, LOW);
173  digitalWrite(watchDogPin, LOW);
174
175  
176  // read+check EEPROM (formatting if new (or not identified))
177  lcd.clear();
178
179  lcd.setCursor(0,0);
180  lcd.print("Memory check...");
181  lcd.setCursor(0,1);
182
183  for (s = 0; s < EEbytes; s ++) {
184    EEaddress = s;
185    EEbytedata = readEEPROM
186  (EEdisk, EEaddress);
187    EEdata[s] = EEbytedata;
188  }
189  if (EEerr) {
190
191    lcd.print("E=");
192    lcd.print(EEerr);
193    delay(1000);
194  }
195  if
196  ((EEdata[0] != EEid1) || (EEdata[1] != EEid2)) {
197    lcd.print(" init! ");
198
199    goupdateEEPROM();
200    if (EEerr) {
201      lcd.print("E=");
202      lcd.print(EEerr);
203
204      delay(1000);
205    }
206  }
207  memcpy(bStearingInterval, EEdata+2, sizeof(int));
208
209  memcpy(bStearingMinToMove, EEdata+4, sizeof(int));
210  memcpy(bStearingMaxMove,
211  EEdata+6, sizeof(int));
212  memcpy(bStearingCoeffMove, EEdata+8, sizeof(float));
213
214  StearingInterval = *((int*) bStearingInterval);
215  StearingMinToMove = *((int*)
216  bStearingMinToMove);
217  StearingMaxMove = *((int*) bStearingMaxMove);
218  StearingCoeffMove
219  = *((float*) bStearingCoeffMove);
220  prev_StearingInterval = StearingInterval;
221
222  prev_StearingMinToMove = StearingMinToMove;
223  prev_StearingMaxMove = StearingMaxMove;
224
225  prev_StearingCoeffMove = StearingCoeffMove;
226  lcd.print(" OK");
227  delay(1000);
228
229  lcd.clear();
230  lcd.print("GPS reading...");
231  delay(1000);
232}
233
234void
235  loop() {
236  // CHECK SENSORS -----------------
237  currCheckSensorsMillis = millis();
238
239  if (currCheckSensorsMillis - prevCheckSensorsMillis >= CheckSensorsInterval) {
240
241    readMuxSensors();
242    if ((SensorVBatt <= 7.0) || (SensorTemp >= 50)) {
243
244      lcd.clear();
245      lcd.setCursor(0,0);
246      lcd.print("Alarm sensors!
247  ");
248      lcd.setCursor(1,1);
249      lcd.print("V=");
250      lcd.print(SensorVBatt);
251
252      lcd.print("  ");
253      lcd.print(int(SensorTemp));
254      lcd.write(0xDF);
255
256      lcd.print("C");
257      NewTone (speakerPin,10);
258      delay(1000);
259
260      noNewTone();
261    }
262    prevCheckSensorsMillis = currCheckSensorsMillis;
263
264  }
265  // STEARING CONTROL ----------------
266  currentStearingMillis = millis();
267
268  if (currentStearingMillis - previousStearingMillis >= StearingInterval) {
269
270    if (isStearing == false && ispause == false) {  // go try (move stearing)
271
272      calcmove = nmf_routetofollow - nmf_truecourse;
273      if (calcmove < (-180))
274  {
275        calcmove = calcmove + 360;
276      } else {
277        if (calcmove
278  > (+180)) {
279          calcmove = calcmove - 360;
280        }
281      }
282
283      if (abs(calcmove) >= StearingMinToMove) {
284        if (abs(calcmove) >=
285  StearingMaxMove) {
286          if (calcmove < 0) {
287              cm = (StearingMaxMove
288  * -1);
289              calcmove = cm;
290            } else {
291              cm
292  = (StearingMaxMove * 1);
293              calcmove = cm;
294          }
295        }
296
297        Stearing = (calcmove * StearingCoeffMove);
298        gomotor(int((Stearing
299  * 216) / 360));  // 54 steps = 1/4 of revolution
300        prevStearing = Stearing;
301
302        isStearing = true;
303      }
304    } else {  // go back (move stearing
305  to "zero" position)
306      if (isStearing == true) {
307        Stearing = (prevStearing
308  * -1);
309        gomotor(int((Stearing * 216) / 360));  // 54 steps = 1/4 of revolution
310
311        Stearing = 0;
312        prevStearing = 0;
313        isStearing = false;
314
315      }
316    }
317    previousStearingMillis = currentStearingMillis;
318  }
319
320  // RC RF BUTTONS ------------------
321  rfRemoteControlValue = checkRfRC();
322
323  if (rfRemoteControlValue) {
324    switch (rfRemoteControlValue) {
325      case
326  RCleftbutton: // Left RC button
327        goleft();
328        break;
329      case
330  RCrightbutton: // Right RC button
331        goright();
332        break;
333      case
334  RCleft10button: // Left-10 RC button
335        goleft10();
336        break;
337
338      case RCright10button: // Right+10 RC button
339        goright10();
340        break;
341
342    }  
343  }  
344  // BUTTONS ------------------------
345  HWButtonValue = checkHWButtons();
346
347  if (HWButtonValue) {
348    switch (HWButtonValue) {
349      case HWleftbutton:
350  // Left(-1) HW button
351        if (isSetup == false) {
352            goleft();
353
354          } else {
355            setupMinus();
356        }
357        break;
358
359      case HWrightbutton: // Right(+1) HW button
360        if (isSetup == false)
361  {
362            goright();
363          } else {
364            setupPlus();
365
366        }
367        break;
368      case HWpausebutton: // Pause HW button
369
370        gopause();
371        break;
372      case HWsetupbutton: // Setup HW button
373
374        gosetup();
375        break;
376      case HWleft10button: // Left(-10)
377  HW button
378        goleft10();
379        break;
380      case HWright10button:
381  // Right(+10) HW button
382        goright10();
383        break;
384    }  
385
386  }
387  // GPS NMEA ------------------
388  if (stringComplete == true) {  // received
389  nmea sentence by serial port RX
390    bool ret;
391    ret = nmeaExtractData();
392
393    inputString = "";
394    stringComplete = false;
395    if (ret == true) {
396
397      RefreshDisplay();
398    }
399  }
400  // WATCHDOG FEEDING ----------------
401
402  if (digitalRead(watchDogPin) == LOW) {
403    digitalWrite(watchDogPin, HIGH);
404
405  } else {
406    digitalWrite(watchDogPin, LOW);
407  }
408}
409
410// read sensors
411  on multiplexer
412void readMuxSensors() {
413  float Vo = 0;
414  float n = 0;
415
416  float n1 = 0;
417  float v1ad = 0;
418  float v2ad = 0;
419  float corr = 0;
420
421  float R1 = 10000;
422  float logR2 = 0;
423  float R2 = 0;
424  float T = 0;
425
426  float c1 = 1.009249522e-03;
427  float c2 = 2.378405444e-04;
428  float c3 = 2.019202697e-07;
429
430  digitalWrite(MuxSelBit0Pin, LOW);  // 00=Vbatt
431  digitalWrite(MuxSelBit1Pin,
432  LOW);
433  n = analogRead(MuxIOPin);
434  v1ad=n;
435  n1=(((10.00 * n) / 1023.00));
436
437  SensorVBatt=(n1 + ((n1 * 0.0) /100));  // arbitrary correction (not active = 0.0%)
438
439  digitalWrite(MuxSelBit0Pin, LOW);  // 01=Vres
440  digitalWrite(MuxSelBit1Pin,
441  HIGH);
442  n = analogRead(MuxIOPin);
443  v2ad=n;
444  n1=(((10.00 * n) / 1023.00));
445
446  SensorVRes=(n1 + ((n1 * 0.0) /100));  // arbitrary correction (not active = 0.0%)
447
448  digitalWrite(MuxSelBit0Pin, HIGH);  // 10=NTC Temp
449  digitalWrite(MuxSelBit1Pin,
450  LOW);
451  Vo = analogRead(MuxIOPin);
452  R2 = R1 * (1023.0 / Vo - 1.0);
453  logR2
454  = log(R2);
455  T = (1.0 / (c1 + c2 * logR2 + c3 * logR2 * logR2 * logR2));
456
457  SensorTemp = T - 273.15;  // Celsius
458  n = (v1ad - v2ad);
459  n1 =  (n / 0.22)
460  * 1000.00;
461  SensormAmp = (((10.00 * n1) / 1023.00));
462}
463
464// extract
465  data from nmea inputString
466bool nmeaExtractData() {
467  bool ret = false;  //true
468  if nmea sentence = $GNRMC and valid CHKSUM
469  if ((inputString.substring(0,6)
470  == "$GNRMC") && (inputString.substring(inputString.length()-4,inputString.length()-2)
471  == nmea0183_checksum(inputString))) {
472    y=0;
473    for (s = 1; s < 11; s ++)
474  { 
475      y=inputString.indexOf(",",y);
476      switch (s) {
477      case
478  1: //time
479        z=inputString.indexOf(",",y+1);
480        if (z>(y+1)) {
481
482          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);
483
484        }
485        y=z;
486        break;
487      case 2: //validity
488        z=inputString.indexOf(",",y+1);
489
490        if (z>(y+1)) {
491          nm_validity=inputString.substring(y+1,y+1+1);
492
493        }
494        y=z;
495        break;
496      case 3: //latitude
497        z=inputString.indexOf(",",y+1);
498
499        if (z>(y+1)) {
500          nm_latitude=inputString.substring(y+1,y+2+1)+""+inputString.substring(y+1+2,y+10+1)+"'";
501
502        }
503        y=z;
504        break;
505      case 4: //north/south
506        z=inputString.indexOf(",",y+1);
507
508        if (z>(y+1)) {
509          nm_latitude=nm_latitude + inputString.substring(y+1,y+1+1);
510
511        }
512        y=z;
513        break;
514      case 5: //longitude
515        z=inputString.indexOf(",",y+1);
516
517        if (z>(y+1)) {
518          nm_longitude=inputString.substring(y+1,y+3+1)+""+inputString.substring(y+1+3,y+11+1)+"'";
519
520        }
521        y=z;
522        break;
523      case 6: //east/west
524        z=inputString.indexOf(",",y+1);
525
526        if (z>(y+1)) {
527          nm_longitude=nm_longitude + inputString.substring(y+1,y+1+1);
528
529        }
530        y=z;
531        break;
532      case 7:  //speed knots
533
534        z=inputString.indexOf(",",y+1);
535        if (z>(y+1)) {
536          nmf_knots=inputString.substring(y+1,z).toFloat();
537
538          t=roundOneDec(nmf_knots);
539          nm_knots=String(t,1)+"kn";
540
541        }
542        y=z;
543        break;
544      case 8: //true course
545        z=inputString.indexOf(",",y+1);
546
547        if (z>(y+1)) {
548          nmf_truecourse=inputString.substring(y+1,z).toFloat();
549
550          d=nmf_truecourse;
551          nm_truecourse=d;
552        }
553        y=z;
554
555        break;
556      case 9: //date
557        z=inputString.indexOf(",",y+1);
558
559        if (z>(y+1)) {
560          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);
561
562        }
563        y=z;
564        break;
565      case 10:
566        // statements
567
568        break;
569      default:
570        // statements
571        break;
572
573      }
574    }
575    if ((isfirstfix == true) || (ispause == true)) {
576      nm_routetofollow=nm_truecourse;
577
578      nmf_routetofollow=nmf_truecourse;
579      isfirstfix=false;
580    }
581
582    ret=true;
583  }
584  return ret;
585}
586
587// increase(+) parameter value
588  during setup
589void setupPlus() {
590  switch (SetupParameter) {
591    case 2:
592  //interval
593      StearingInterval = (StearingInterval + 100);
594      if (StearingInterval
595  > 5000) {
596        StearingInterval = 5000;
597      }
598      break;
599    case
600  3: //min. to move
601      StearingMinToMove = (StearingMinToMove + 1);
602      if
603  (StearingMinToMove > 20) {
604        StearingMinToMove = 20;
605      }
606      break;
607
608    case 4: //max. move
609      StearingMaxMove = (StearingMaxMove + 1);
610      if
611  (StearingMaxMove > 45) {
612        StearingMaxMove = 45;
613      }
614      break;
615
616    case 5: //coefficient
617      StearingCoeffMove = (StearingCoeffMove + 0.1);
618
619      if (StearingCoeffMove > 4) {
620        StearingCoeffMove = 4;
621      }
622
623      break;
624  }
625  delay(200);
626  RefreshDisplay();
627}
628
629// decrease(-)
630  parameter value during setup
631void setupMinus() {
632  switch (SetupParameter)
633  {
634    case 2: //interval
635      StearingInterval = (StearingInterval - 100);
636
637      if (StearingInterval < 1000) {
638        StearingInterval = 1000;
639      }
640
641      break;
642    case 3: //min. to move
643      StearingMinToMove = (StearingMinToMove
644  - 1);
645      if (StearingMinToMove < 0) {
646        StearingMinToMove = 0;
647
648      }
649      break;
650    case 4: //max. move
651      StearingMaxMove = (StearingMaxMove
652  - 1);
653      if (StearingMaxMove < 10) {
654        StearingMaxMove = 10;
655
656      }
657      break;
658    case 5: //coefficient
659      StearingCoeffMove
660  = (StearingCoeffMove - 0.1);
661      if (StearingCoeffMove < 0.1) {
662        StearingCoeffMove
663  = 0.1;
664      }
665      break;
666  }
667  delay(200);
668  RefreshDisplay();
669}
670
671//
672  motor control (+)=forward (-)=backwards
673void gomotor(int stepsToMove) {
674  digitalWrite(motorsABenablePin,
675  HIGH);
676  motor.step(stepsToMove);
677  digitalWrite(motorsABenablePin, LOW);
678}
679
680//
681  refresh data on display
682void RefreshDisplay() {
683  if (isSetup == false) {
684  //---------normal
685    lcd.clear();
686    lcd.setCursor(0,0);
687    lcd.print("R"+nm_routetofollow);
688
689    lcd.write(0xDF);
690    lcd.print(" H"+nm_truecourse);
691    lcd.write(0xDF);
692
693    if (ispause == true) {
694      lcd.print(" STOP");    
695    } else {
696
697      if (Stearing > 0) {
698        lcd.print(" +");
699      }
700      if (Stearing
701  == 0) {
702        lcd.print("  ");
703      }
704      if (Stearing < 0) {
705
706        lcd.print(" ");
707      }
708      lcd.print(int(Stearing));
709    }
710
711    lcd.setCursor(0,1);
712    lcd.print(nm_time+" "+nm_knots);
713  }
714  if
715  (isSetup == true) {  //-----------setup
716    lcd.clear();
717    lcd.setCursor(0,0);
718
719    lcd.print("setup: ");
720    switch (SetupParameter) {
721      case 1: //display
722  sensors
723        readMuxSensors(); 
724        lcd.print("V=");
725        lcd.print(SensorVBatt);
726
727        lcd.setCursor(1,1);
728        lcd.print("mA=");
729        lcd.print(int(SensormAmp));
730
731        lcd.print("  ");
732        lcd.print(int(SensorTemp));
733        lcd.write(0xDF);
734
735        lcd.print("C");
736        break;
737      case 2: //interval
738        lcd.print("interval");
739
740        lcd.setCursor(7,1);
741        lcd.print(StearingInterval);
742        lcd.print("
743  mSec");
744        break;
745      case 3: //min. to move
746        lcd.print("minimum");
747
748        lcd.setCursor(7,1);
749        lcd.print(StearingMinToMove);
750        lcd.write(0xDF);
751
752        break;
753      case 4: //max. move
754        lcd.print("max");
755        lcd.setCursor(7,1);
756
757        lcd.print(StearingMaxMove);
758        lcd.write(0xDF);
759       break;
760
761      case 5: //coefficient
762        lcd.print("coeffic.");
763        lcd.setCursor(7,1);
764
765        lcd.print(StearingCoeffMove);
766        lcd.print(" x ");
767        lcd.write(0xDF);
768
769        break;
770    }
771  }
772}
773/*
774  SerialEvent occurs whenever a new
775  data comes in the hardware serial RX. This
776  routine is run between each time
777  loop() runs, so using delay inside loop can
778  delay response. Multiple bytes
779  of data may be available.
780*/
781void serialEvent() {
782  while (Serial.available())
783  {
784    char inChar = (char)Serial.read();
785    inputString += inChar;
786    //
787  if the incoming character is a newline, set a flag so the main loop can
788    //
789  do something about it
790    if (inChar == '\
791') {
792      stringComplete = true;
793
794      }
795    }
796  }
797
798//calculate checksum of nmea sentence
799String nmea0183_checksum(String
800  nmea_data) {
801    int crc = 0;
802    String chSumString = "";
803    int i;
804
805    // ignore the first $ sign, checksum in sentence
806    for (i = 1; i < (nmea_data.length()-5);
807  i ++) { // remove the - 5 if no "*" + cksum + cr + lf are present
808        crc
809  ^= nmea_data[i];
810    }
811    chSumString = String(crc,HEX);
812    if (chSumString.length()==1)
813  {
814      chSumString="0"+chSumString.substring(0,1);
815    }
816    chSumString.toUpperCase();
817
818    return chSumString;
819}
820
821//check RC which button is pressed
822int checkRfRC()
823  {
824  int n = 0;
825  int res = 0;
826  n = analogRead(rfRemoteControlPin);
827
828  if ((n>350) and (n<460)) { // button A
829    res = RCleftbutton;
830  }
831  if
832  ((n> 90) and (n<190)) { // button B
833    res = RCrightbutton;
834  }
835  if ((n>540)
836  and (n<640)) { // button C
837    res = RCleft10button;
838  }
839  if ((n>225)
840  and (n<325)) { // button D
841    res = RCright10button;
842  }
843  return res;
844    
845}
846
847//check HW which button is pressed
848int checkHWButtons() {
849
850  int n = 0;
851  int res = 0;
852  n = analogRead(ButtonsPin);  
853  //Serial.println(n);
854  
855  if ((n>465) and (n<565)) { // button left
856    res = HWleftbutton;
857  }
858
859  if ((n>290) and (n<390)) { // button right
860    res = HWrightbutton;
861  }
862
863  if ((n>130) and (n<220)) { // button pause
864    res = HWpausebutton;
865  }
866
867  if ((n>625) and (n<725)) { // button setup
868    res = HWsetupbutton;
869  }
870
871  if ((n>975) and (n<1075)) { // button left-10
872    res = HWleft10button;
873
874  }
875  if ((n>800) and (n<900)) { // button right+10
876    res = HWright10button;
877
878  }
879  return res;    
880}
881
882void gosetup() {  // setup button
883  if (isSetup
884  == false) {
885      SetupParameter = 1;
886      isSetup = true;
887    } else
888  {
889      if (SetupParameter < 5) {
890          SetupParameter ++;
891        }
892  else {
893          if (prev_StearingInterval != StearingInterval || prev_StearingMinToMove
894  != StearingMinToMove || prev_StearingMaxMove != StearingMaxMove || prev_StearingCoeffMove
895  != StearingCoeffMove) {
896            lcd.clear();
897            lcd.setCursor(0,0);
898
899            lcd.print("updating... ");
900            delay(1000);
901            goupdateEEPROM();
902
903            if (EEerr) {
904              lcd.print("E=");
905              lcd.print(EEerr);
906
907              delay(1000);
908            }
909            prev_StearingInterval
910  = StearingInterval;
911            prev_StearingMinToMove = StearingMinToMove;
912
913            prev_StearingMaxMove = StearingMaxMove;
914            prev_StearingCoeffMove
915  = StearingCoeffMove;
916          }
917          isSetup = false;
918      }
919
920  }
921  NewTone (speakerPin,2000);
922  delay(200);
923  noNewTone();
924  RefreshDisplay();
925}
926
927void
928  goupdateEEPROM() {
929  EEaddress = 0;  //id1
930  EEdata[0] = EEid1;
931  EEbytedata
932  = EEid1;
933  writeEEPROM (EEdisk, EEaddress, EEbytedata);
934  EEaddress = 1;  //id2
935
936  EEdata[1] = EEid2;
937  EEbytedata = EEid2;
938  writeEEPROM (EEdisk, EEaddress,
939  EEbytedata);
940  memcpy(bStearingInterval, &StearingInterval, sizeof(int));
941
942  memcpy(bStearingMinToMove, &StearingMinToMove, sizeof(int));
943  memcpy(bStearingMaxMove,
944  &StearingMaxMove, sizeof(int));
945  memcpy(bStearingCoeffMove, &StearingCoeffMove,
946  sizeof(float));
947  memcpy(EEdata+2,bStearingInterval,sizeof(int));
948  memcpy(EEdata+4,bStearingMinToMove,sizeof(int));
949
950  memcpy(EEdata+6,bStearingMaxMove,sizeof(int));
951  memcpy(EEdata+8,bStearingCoeffMove,sizeof(float));
952
953  for (s = 2; s < EEbytes; s ++) {
954    EEaddress = s;  //data
955    EEbytedata
956  = EEdata[s];
957    writeEEPROM (EEdisk, EEaddress, EEbytedata);
958  }
959}
960
961void
962  goleft() {  // left button/RC
963  if (ispause == false) {
964    nmf_routetofollow
965  --;
966    if (nmf_routetofollow < 1) {
967      nmf_routetofollow = 360;
968    }
969
970    d=nmf_routetofollow;
971    nmf_routetofollow=d;
972    nm_routetofollow=d;
973
974    NewTone (speakerPin,400);
975    delay(200);
976    noNewTone();
977  } else
978  {
979    NewTone (speakerPin,1000);
980    delay(50);
981    noNewTone();
982  }
983
984  RefreshDisplay();
985}
986
987void goleft10() {  // left 10x button/RC
988  if
989  (ispause == false) {
990    for (s = 1; s < 11; s ++) {
991      nmf_routetofollow
992  --;
993      if (nmf_routetofollow < 1) {
994        nmf_routetofollow = 360;
995
996      }
997    }
998    d=nmf_routetofollow;
999    nmf_routetofollow=d;
1000    nm_routetofollow=d;
1001
1002    NewTone (speakerPin,400);
1003    delay(200);
1004    noNewTone();
1005  } else
1006  {
1007    NewTone (speakerPin,1000);
1008    delay(50);
1009    noNewTone();
1010  }
1011
1012  RefreshDisplay();
1013}
1014
1015void goright() {  // right button/RC
1016  if (ispause
1017  == false) {
1018    nmf_routetofollow ++;
1019    if (nmf_routetofollow > 360) {
1020
1021      nmf_routetofollow = 1;
1022    }
1023    d=nmf_routetofollow;
1024    nmf_routetofollow=d;
1025
1026    nm_routetofollow=d;
1027    NewTone (speakerPin,800);
1028    delay(200);
1029
1030    noNewTone();
1031  } else {
1032    NewTone (speakerPin,1000);
1033    delay(50);
1034
1035    noNewTone();
1036  }
1037  RefreshDisplay();
1038}
1039
1040void goright10() {  //
1041  right 10x button/RC
1042  if (ispause == false) {
1043    for (s = 1; s < 11; s ++)
1044  {
1045      nmf_routetofollow ++;
1046      if (nmf_routetofollow > 360) {
1047        nmf_routetofollow
1048  = 1;
1049      }
1050    }
1051    d=nmf_routetofollow;
1052    nmf_routetofollow=d;
1053
1054    nm_routetofollow=d;
1055    NewTone (speakerPin,800);
1056    delay(200);
1057
1058    noNewTone();
1059  } else {
1060    NewTone (speakerPin,1000);
1061    delay(50);
1062
1063    noNewTone();
1064  }
1065  RefreshDisplay();
1066}
1067
1068void gopause() {  //
1069  pause button/RC
1070  if (ispause == true) {
1071    ispause=false; 
1072    digitalWrite(ledpausePin,
1073  HIGH);
1074    NewTone (speakerPin,50);
1075    delay(200);
1076    NewTone (speakerPin,200);
1077
1078    delay(800);
1079    noNewTone();
1080  } else {
1081    ispause=true; 
1082    digitalWrite(ledpausePin,
1083  LOW);
1084    NewTone (speakerPin,200);
1085    delay(200);
1086    NewTone (speakerPin,50);
1087
1088    delay(800);
1089    noNewTone();
1090  }
1091  RefreshDisplay();
1092}
1093
1094//
1095  reading eeprom
1096byte readEEPROM (int diskaddress, unsigned int memaddress) {
1097
1098  byte rdata = 0x00;
1099  Wire.beginTransmission (diskaddress);
1100  Wire.write
1101  (memaddress);
1102  if (Wire.endTransmission () == 0) {
1103    Wire.requestFrom (diskaddress,1);
1104
1105    if (Wire.available()) {
1106        rdata = Wire.read();
1107      } else {
1108
1109        EEerr = 1; //"READ no data available"
1110    }
1111    } else {
1112      EEerr
1113  = 2; //"READ eTX error"
1114  }
1115  Wire.endTransmission (true);
1116  return rdata;
1117}
1118
1119//
1120  writing eeprom
1121void writeEEPROM (int diskaddress, unsigned int memaddress, byte
1122  bytedata) {
1123  Wire.beginTransmission (diskaddress);
1124  Wire.write (memaddress);
1125
1126  Wire.write (bytedata);
1127  if (Wire.endTransmission () != 0) {
1128    EEerr =
1129  3; //"WRITING eTX error"
1130  }
1131  Wire.endTransmission (true);
1132  delay(5);
1133  
1134}
1135
1136// round zero decimal
1137float roundZeroDec(float f) {
1138  float y,
1139  d;
1140  y = f*1;
1141  d = y - (int)y;
1142  y = (float)(int)(f*1)/1;
1143  if (d >=
1144  0.5) {
1145    y += 1;
1146   } else {
1147    if (d < -0.5) {
1148    y -= 1;
1149  }
1150
1151  }
1152  return y;
1153}
1154
1155// round one decimal
1156float roundOneDec(float f)
1157  {
1158  float y, d;
1159  y = f*10;
1160  d = y - (int)y;
1161  y = (float)(int)(f*10)/10;
1162
1163  if (d >= 0.5) {
1164    y += 0.1;
1165   } else {
1166    if (d < -0.5) {
1167    y
1168  -= 0.1;
1169  }
1170  }
1171  return y;
1172}
1173
1174// round two decimal
1175float roundTwoDec(float
1176  f) {
1177  float y, d;
1178  y = f*100;
1179  d = y - (int)y;
1180  y = (float)(int)(f*100)/100;
1181
1182  if (d >= 0.5) {
1183    y += 0.01;
1184   } else {
1185    if (d < -0.5) {
1186    y
1187  -= 0.01;
1188  }
1189  }
1190  return y;
1191}
1192

1/*
2 * This sketch is a Watchdog to keep CLIENT under control, on Arduino
3  NANO 3.0 by Marco Zonca
4 * CLIENT must feed Whatcdog sooner then feedingInterval
5  otherwise will be forced to restart
6 * 
7 */
8
9const int feedingPin =
10  14;
11const int ledPin =  15;
12const int restartPin = 16;
13const int buzzerPin
14  = 17;
15const long ledInterval = 1000;
16const long feedingInterval = 2500;
17const
18  long timeForClientStart = 16000;
19
20int ledState = LOW;
21int previousFeedingState
22  = LOW;
23int feedingState = LOW;
24unsigned long previousLedMillis = 0;
25unsigned
26  long previousFeedingMillis = 0;
27
28void setup() {
29  digitalWrite(restartPin,
30  HIGH);  // LOW will force CLIENT to restart
31  pinMode(ledPin, OUTPUT);
32  pinMode(buzzerPin,
33  OUTPUT);
34  pinMode(restartPin, OUTPUT);
35  pinMode(feedingPin, INPUT);
36
37  delay(timeForClientStart);  // let time to CLIENT to start...
38}
39
40void
41  loop() {
42  unsigned long currentMillis = millis();
43  // BLINK LED -------------------
44
45  if (currentMillis - previousLedMillis >= ledInterval) {
46    previousLedMillis
47  = currentMillis;
48    if (ledState == LOW) {
49      ledState = HIGH;
50    }
51  else {
52      ledState = LOW;
53    }
54    digitalWrite(ledPin, ledState);
55
56  }
57  // CHECK THE FEEDING -------------------
58  feedingState = digitalRead(feedingPin);
59  // CLIENT must set pin HIGH -> LOW frequently to prove it's alive
60  if (feedingState
61  == HIGH) {
62    if (previousFeedingState == LOW) {
63      previousFeedingMillis
64  = currentMillis;
65    }
66    previousFeedingState = HIGH;
67  } else {
68
69    previousFeedingState = LOW;
70  }
71  if (currentMillis - previousFeedingMillis
72  > feedingInterval) {  // CLIENT is sleeping
73    ledState = HIGH;
74    digitalWrite(ledPin,
75  ledState);
76    tone(buzzerPin,1500);
77    delay(500);
78    digitalWrite(restartPin,
79  LOW);  //restart CLIENT
80    tone(buzzerPin,1500);
81    delay(500);
82    digitalWrite(restartPin,
83  HIGH);
84    tone(buzzerPin,1500);
85    delay(timeForClientStart);  // let CLIENT
86  time to restart...
87    noTone(buzzerPin);
88    currentMillis = millis();
89
90    previousFeedingState = LOW;
91    previousFeedingMillis = currentMillis;
92
93    previousLedMillis = currentMillis;
94  }
95}
96

1/*
2  This sketch act as Autopilot for small sailing boats, by Marco  Zonca, 2019
3  Arduino UNO as CPU, Arduino Nano as watchdog, GPS BT-220 nmea,  stepper motor + controller, rf433Mhz RC, 6 buttons, buzzer, i2c display,
4  2  leds, i2c 24c04 eeprom, Mux 4051 for sensors, lipo 2s 7.4v 2600mA, 7805 voltage  regulator, Thermistor;
5*/
6
7#include <LiquidCrystal_I2C.h>
8#include <NewTone.h>
9#include  <Stepper.h>
10#include <Wire.h>
11
12String inputString = "";
13String nm_time  = "00:00:00";
14String nm_validity = "V";
15String nm_latitude = "ddmm.mmmm'N";
16String  nm_longitude = "dddmm.mmmm'E";
17String nm_knots = "0.0kn";
18float nmf_knots  = 0.0;
19String nm_truecourse = "360";
20float nmf_truecourse = 360;
21String  nm_date = "dd/mm/yyyy";
22String nm_routetofollow ="000";
23float nmf_routetofollow  = 0;
24unsigned long previousStearingMillis = 0;
25unsigned long currentStearingMillis  = 0;
26unsigned long prevCheckSensorsMillis = 0;
27unsigned long currCheckSensorsMillis  = 0;
28int CheckSensorsInterval = 10000;
29
30bool stringComplete = false;
31bool  isfirstfix = true;
32bool ispause = true;
33bool isStearing = false;
34bool isSetup  = false;
35
36int s=0;
37int y=0;
38int z=0;
39int d=0;
40int rfRemoteControlValue  = 0;
41int HWButtonValue = 0;
42int SetupParameter = 0;
43float calcmove = 0;
44float  cm = 0;
45float Stearing = 0;
46float prevStearing = 0;
47float t = 0;
48int  EEdisk = 0x50;
49int EEid1 = 0x29;
50int EEid2 = 0x00;
51unsigned int EEaddress  = 0;
52unsigned int EEbytes = 12;
53byte EEdata[12];
54byte EEbytedata;
55int  EEerr = 0;
56float SensorVBatt=0;
57float SensorVRes=0;
58float SensorTemp=0;
59float  SensormAmp=0;
60
61// following parameters are the defaults but are read/write  in eeprom
62// eeprom is initialized if at addresses 0 and 1 the content is different       addres  len   type    notes
63// 0-255 bytes at 0x50 EEdisk, 256-512 bytes  at 0x51 (not used)                ---------------------------------------------------------------
64//                                                                              0       1B    byte    01001001 (0x29 as autopilot project id1)
65//                                                                              1       1B    byte    00000000 (0x00       "          "   id2)
66int StearingInterval  = 2000;  // millis between try and back                    2       2B    int     StearingInterval  1000-5000 steps 100
67int StearingMinToMove = 2;  // compass_degrees                                  4       2B    int     StearingMinToMove    0-20   steps   1
68int StearingMaxMove  = 40;  // compass_degrees                                   6       2B    int     StearingMaxMove     10-45   steps   1
69float StearingCoeffMove = 1.5;  // used as (compass_degrees  * coeff)            8       4B    float   StearingCoeffMove  0.1-4    steps 0.1
70//                                                                              12      free
71//
72byte bStearingInterval[sizeof(int)];
73byte bStearingMinToMove[sizeof(int)];
74byte  bStearingMaxMove[sizeof(int)];
75byte bStearingCoeffMove[sizeof(float)];
76int  prev_StearingInterval=0;
77int prev_StearingMinToMove=0;
78int prev_StearingMaxMove=0;
79float  prev_StearingCoeffMove=0;
80
81const int ledpausePin = 2;
82const int watchDogPin  = 3;
83const int MuxSelBit0Pin = 7;  // 00=Vin 01=Vbatt 10=Temp
84const int MuxSelBit1Pin  = 6;  // 
85const int motorsABenablePin = 13;
86const int MuxIOPin = 14;
87const  int ButtonsPin = 15;
88const int rfRemoteControlPin = 16;
89const int speakerPin  = 17;
90const int RCleftbutton = 201;
91const int RCrightbutton = 202;
92const  int RCleft10button = 203;
93const int RCright10button = 204;
94const int HWleftbutton  = 101;
95const int HWrightbutton = 102;
96const int HWpausebutton = 103;
97const  int HWsetupbutton = 104;
98const int HWleft10button = 105;
99const int HWright10button  = 106;
100const int motorStepsPerRevolution = 200;  // 200 for model 23LM, 54 steps  = 1/4 of revolution
101
102LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7, 3,  POSITIVE);
103Stepper motor(motorStepsPerRevolution, 9, 10, 11, 12);
104
105void  setup() {
106  Serial.begin(4800);
107  lcd.begin(16,2);
108  Wire.begin();
109  motor.setSpeed(60);
110  inputString.reserve(200);
111  pinMode(motorsABenablePin,  OUTPUT);
112  pinMode(MuxSelBit0Pin, OUTPUT);
113  pinMode(MuxSelBit1Pin, OUTPUT);
114  digitalWrite(motorsABenablePin, LOW);
115  digitalWrite(MuxSelBit0Pin, LOW);
116  digitalWrite(MuxSelBit1Pin, LOW);
117  pinMode(ledpausePin, OUTPUT);
118  pinMode(watchDogPin,  OUTPUT);
119  digitalWrite(ledpausePin, LOW);
120  digitalWrite(watchDogPin, LOW);
121  
122  // read+check EEPROM (formatting if new (or not identified))
123  lcd.clear();
124  lcd.setCursor(0,0);
125  lcd.print("Memory check...");
126  lcd.setCursor(0,1);
127  for (s = 0; s < EEbytes; s ++) {
128    EEaddress = s;
129    EEbytedata = readEEPROM  (EEdisk, EEaddress);
130    EEdata[s] = EEbytedata;
131  }
132  if (EEerr) {
133    lcd.print("E=");
134    lcd.print(EEerr);
135    delay(1000);
136  }
137  if  ((EEdata[0] != EEid1) || (EEdata[1] != EEid2)) {
138    lcd.print(" init! ");
139    goupdateEEPROM();
140    if (EEerr) {
141      lcd.print("E=");
142      lcd.print(EEerr);
143      delay(1000);
144    }
145  }
146  memcpy(bStearingInterval, EEdata+2, sizeof(int));
147  memcpy(bStearingMinToMove, EEdata+4, sizeof(int));
148  memcpy(bStearingMaxMove,  EEdata+6, sizeof(int));
149  memcpy(bStearingCoeffMove, EEdata+8, sizeof(float));
150  StearingInterval = *((int*) bStearingInterval);
151  StearingMinToMove = *((int*)  bStearingMinToMove);
152  StearingMaxMove = *((int*) bStearingMaxMove);
153  StearingCoeffMove  = *((float*) bStearingCoeffMove);
154  prev_StearingInterval = StearingInterval;
155  prev_StearingMinToMove = StearingMinToMove;
156  prev_StearingMaxMove = StearingMaxMove;
157  prev_StearingCoeffMove = StearingCoeffMove;
158  lcd.print(" OK");
159  delay(1000);
160  lcd.clear();
161  lcd.print("GPS reading...");
162  delay(1000);
163}
164
165void  loop() {
166  // CHECK SENSORS -----------------
167  currCheckSensorsMillis = millis();
168  if (currCheckSensorsMillis - prevCheckSensorsMillis >= CheckSensorsInterval) {
169    readMuxSensors();
170    if ((SensorVBatt <= 7.0) || (SensorTemp >= 50)) {
171      lcd.clear();
172      lcd.setCursor(0,0);
173      lcd.print("Alarm sensors!  ");
174      lcd.setCursor(1,1);
175      lcd.print("V=");
176      lcd.print(SensorVBatt);
177      lcd.print("  ");
178      lcd.print(int(SensorTemp));
179      lcd.write(0xDF);
180      lcd.print("C");
181      NewTone (speakerPin,10);
182      delay(1000);
183      noNewTone();
184    }
185    prevCheckSensorsMillis = currCheckSensorsMillis;
186  }
187  // STEARING CONTROL ----------------
188  currentStearingMillis = millis();
189  if (currentStearingMillis - previousStearingMillis >= StearingInterval) {
190    if (isStearing == false && ispause == false) {  // go try (move stearing)
191      calcmove = nmf_routetofollow - nmf_truecourse;
192      if (calcmove < (-180))  {
193        calcmove = calcmove + 360;
194      } else {
195        if (calcmove  > (+180)) {
196          calcmove = calcmove - 360;
197        }
198      }
199      if (abs(calcmove) >= StearingMinToMove) {
200        if (abs(calcmove) >=  StearingMaxMove) {
201          if (calcmove < 0) {
202              cm = (StearingMaxMove  * -1);
203              calcmove = cm;
204            } else {
205              cm  = (StearingMaxMove * 1);
206              calcmove = cm;
207          }
208        }
209        Stearing = (calcmove * StearingCoeffMove);
210        gomotor(int((Stearing  * 216) / 360));  // 54 steps = 1/4 of revolution
211        prevStearing = Stearing;
212        isStearing = true;
213      }
214    } else {  // go back (move stearing  to "zero" position)
215      if (isStearing == true) {
216        Stearing = (prevStearing  * -1);
217        gomotor(int((Stearing * 216) / 360));  // 54 steps = 1/4 of revolution
218        Stearing = 0;
219        prevStearing = 0;
220        isStearing = false;
221      }
222    }
223    previousStearingMillis = currentStearingMillis;
224  }
225  // RC RF BUTTONS ------------------
226  rfRemoteControlValue = checkRfRC();
227  if (rfRemoteControlValue) {
228    switch (rfRemoteControlValue) {
229      case  RCleftbutton: // Left RC button
230        goleft();
231        break;
232      case  RCrightbutton: // Right RC button
233        goright();
234        break;
235      case  RCleft10button: // Left-10 RC button
236        goleft10();
237        break;
238      case RCright10button: // Right+10 RC button
239        goright10();
240        break;
241    }  
242  }  
243  // BUTTONS ------------------------
244  HWButtonValue = checkHWButtons();
245  if (HWButtonValue) {
246    switch (HWButtonValue) {
247      case HWleftbutton:  // Left(-1) HW button
248        if (isSetup == false) {
249            goleft();
250          } else {
251            setupMinus();
252        }
253        break;
254      case HWrightbutton: // Right(+1) HW button
255        if (isSetup == false)  {
256            goright();
257          } else {
258            setupPlus();
259        }
260        break;
261      case HWpausebutton: // Pause HW button
262        gopause();
263        break;
264      case HWsetupbutton: // Setup HW button
265        gosetup();
266        break;
267      case HWleft10button: // Left(-10)  HW button
268        goleft10();
269        break;
270      case HWright10button:  // Right(+10) HW button
271        goright10();
272        break;
273    }  
274  }
275  // GPS NMEA ------------------
276  if (stringComplete == true) {  // received  nmea sentence by serial port RX
277    bool ret;
278    ret = nmeaExtractData();
279    inputString = "";
280    stringComplete = false;
281    if (ret == true) {
282      RefreshDisplay();
283    }
284  }
285  // WATCHDOG FEEDING ----------------
286  if (digitalRead(watchDogPin) == LOW) {
287    digitalWrite(watchDogPin, HIGH);
288  } else {
289    digitalWrite(watchDogPin, LOW);
290  }
291}
292
293// read sensors  on multiplexer
294void readMuxSensors() {
295  float Vo = 0;
296  float n = 0;
297  float n1 = 0;
298  float v1ad = 0;
299  float v2ad = 0;
300  float corr = 0;
301  float R1 = 10000;
302  float logR2 = 0;
303  float R2 = 0;
304  float T = 0;
305  float c1 = 1.009249522e-03;
306  float c2 = 2.378405444e-04;
307  float c3 = 2.019202697e-07;
308  digitalWrite(MuxSelBit0Pin, LOW);  // 00=Vbatt
309  digitalWrite(MuxSelBit1Pin,  LOW);
310  n = analogRead(MuxIOPin);
311  v1ad=n;
312  n1=(((10.00 * n) / 1023.00));
313  SensorVBatt=(n1 + ((n1 * 0.0) /100));  // arbitrary correction (not active = 0.0%)
314  digitalWrite(MuxSelBit0Pin, LOW);  // 01=Vres
315  digitalWrite(MuxSelBit1Pin,  HIGH);
316  n = analogRead(MuxIOPin);
317  v2ad=n;
318  n1=(((10.00 * n) / 1023.00));
319  SensorVRes=(n1 + ((n1 * 0.0) /100));  // arbitrary correction (not active = 0.0%)
320  digitalWrite(MuxSelBit0Pin, HIGH);  // 10=NTC Temp
321  digitalWrite(MuxSelBit1Pin,  LOW);
322  Vo = analogRead(MuxIOPin);
323  R2 = R1 * (1023.0 / Vo - 1.0);
324  logR2  = log(R2);
325  T = (1.0 / (c1 + c2 * logR2 + c3 * logR2 * logR2 * logR2));
326  SensorTemp = T - 273.15;  // Celsius
327  n = (v1ad - v2ad);
328  n1 =  (n / 0.22)  * 1000.00;
329  SensormAmp = (((10.00 * n1) / 1023.00));
330}
331
332// extract  data from nmea inputString
333bool nmeaExtractData() {
334  bool ret = false;  //true  if nmea sentence = $GNRMC and valid CHKSUM
335  if ((inputString.substring(0,6)  == "$GNRMC") && (inputString.substring(inputString.length()-4,inputString.length()-2)  == nmea0183_checksum(inputString))) {
336    y=0;
337    for (s = 1; s < 11; s ++)  { 
338      y=inputString.indexOf(",",y);
339      switch (s) {
340      case  1: //time
341        z=inputString.indexOf(",",y+1);
342        if (z>(y+1)) {
343          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);
344        }
345        y=z;
346        break;
347      case 2: //validity
348        z=inputString.indexOf(",",y+1);
349        if (z>(y+1)) {
350          nm_validity=inputString.substring(y+1,y+1+1);
351        }
352        y=z;
353        break;
354      case 3: //latitude
355        z=inputString.indexOf(",",y+1);
356        if (z>(y+1)) {
357          nm_latitude=inputString.substring(y+1,y+2+1)+""+inputString.substring(y+1+2,y+10+1)+"'";
358        }
359        y=z;
360        break;
361      case 4: //north/south
362        z=inputString.indexOf(",",y+1);
363        if (z>(y+1)) {
364          nm_latitude=nm_latitude + inputString.substring(y+1,y+1+1);
365        }
366        y=z;
367        break;
368      case 5: //longitude
369        z=inputString.indexOf(",",y+1);
370        if (z>(y+1)) {
371          nm_longitude=inputString.substring(y+1,y+3+1)+""+inputString.substring(y+1+3,y+11+1)+"'";
372        }
373        y=z;
374        break;
375      case 6: //east/west
376        z=inputString.indexOf(",",y+1);
377        if (z>(y+1)) {
378          nm_longitude=nm_longitude + inputString.substring(y+1,y+1+1);
379        }
380        y=z;
381        break;
382      case 7:  //speed knots
383        z=inputString.indexOf(",",y+1);
384        if (z>(y+1)) {
385          nmf_knots=inputString.substring(y+1,z).toFloat();
386          t=roundOneDec(nmf_knots);
387          nm_knots=String(t,1)+"kn";
388        }
389        y=z;
390        break;
391      case 8: //true course
392        z=inputString.indexOf(",",y+1);
393        if (z>(y+1)) {
394          nmf_truecourse=inputString.substring(y+1,z).toFloat();
395          d=nmf_truecourse;
396          nm_truecourse=d;
397        }
398        y=z;
399        break;
400      case 9: //date
401        z=inputString.indexOf(",",y+1);
402        if (z>(y+1)) {
403          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);
404        }
405        y=z;
406        break;
407      case 10:
408        // statements
409        break;
410      default:
411        // statements
412        break;
413      }
414    }
415    if ((isfirstfix == true) || (ispause == true)) {
416      nm_routetofollow=nm_truecourse;
417      nmf_routetofollow=nmf_truecourse;
418      isfirstfix=false;
419    }
420    ret=true;
421  }
422  return ret;
423}
424
425// increase(+) parameter value  during setup
426void setupPlus() {
427  switch (SetupParameter) {
428    case 2:  //interval
429      StearingInterval = (StearingInterval + 100);
430      if (StearingInterval  > 5000) {
431        StearingInterval = 5000;
432      }
433      break;
434    case  3: //min. to move
435      StearingMinToMove = (StearingMinToMove + 1);
436      if  (StearingMinToMove > 20) {
437        StearingMinToMove = 20;
438      }
439      break;
440    case 4: //max. move
441      StearingMaxMove = (StearingMaxMove + 1);
442      if  (StearingMaxMove > 45) {
443        StearingMaxMove = 45;
444      }
445      break;
446    case 5: //coefficient
447      StearingCoeffMove = (StearingCoeffMove + 0.1);
448      if (StearingCoeffMove > 4) {
449        StearingCoeffMove = 4;
450      }
451      break;
452  }
453  delay(200);
454  RefreshDisplay();
455}
456
457// decrease(-)  parameter value during setup
458void setupMinus() {
459  switch (SetupParameter)  {
460    case 2: //interval
461      StearingInterval = (StearingInterval - 100);
462      if (StearingInterval < 1000) {
463        StearingInterval = 1000;
464      }
465      break;
466    case 3: //min. to move
467      StearingMinToMove = (StearingMinToMove  - 1);
468      if (StearingMinToMove < 0) {
469        StearingMinToMove = 0;
470      }
471      break;
472    case 4: //max. move
473      StearingMaxMove = (StearingMaxMove  - 1);
474      if (StearingMaxMove < 10) {
475        StearingMaxMove = 10;
476      }
477      break;
478    case 5: //coefficient
479      StearingCoeffMove  = (StearingCoeffMove - 0.1);
480      if (StearingCoeffMove < 0.1) {
481        StearingCoeffMove  = 0.1;
482      }
483      break;
484  }
485  delay(200);
486  RefreshDisplay();
487}
488
489//  motor control (+)=forward (-)=backwards
490void gomotor(int stepsToMove) {
491  digitalWrite(motorsABenablePin,  HIGH);
492  motor.step(stepsToMove);
493  digitalWrite(motorsABenablePin, LOW);
494}
495
496//  refresh data on display
497void RefreshDisplay() {
498  if (isSetup == false) {  //---------normal
499    lcd.clear();
500    lcd.setCursor(0,0);
501    lcd.print("R"+nm_routetofollow);
502    lcd.write(0xDF);
503    lcd.print(" H"+nm_truecourse);
504    lcd.write(0xDF);
505    if (ispause == true) {
506      lcd.print(" STOP");    
507    } else {
508      if (Stearing > 0) {
509        lcd.print(" +");
510      }
511      if (Stearing  == 0) {
512        lcd.print("  ");
513      }
514      if (Stearing < 0) {
515        lcd.print(" ");
516      }
517      lcd.print(int(Stearing));
518    }
519    lcd.setCursor(0,1);
520    lcd.print(nm_time+" "+nm_knots);
521  }
522  if  (isSetup == true) {  //-----------setup
523    lcd.clear();
524    lcd.setCursor(0,0);
525    lcd.print("setup: ");
526    switch (SetupParameter) {
527      case 1: //display  sensors
528        readMuxSensors(); 
529        lcd.print("V=");
530        lcd.print(SensorVBatt);
531        lcd.setCursor(1,1);
532        lcd.print("mA=");
533        lcd.print(int(SensormAmp));
534        lcd.print("  ");
535        lcd.print(int(SensorTemp));
536        lcd.write(0xDF);
537        lcd.print("C");
538        break;
539      case 2: //interval
540        lcd.print("interval");
541        lcd.setCursor(7,1);
542        lcd.print(StearingInterval);
543        lcd.print("  mSec");
544        break;
545      case 3: //min. to move
546        lcd.print("minimum");
547        lcd.setCursor(7,1);
548        lcd.print(StearingMinToMove);
549        lcd.write(0xDF);
550        break;
551      case 4: //max. move
552        lcd.print("max");
553        lcd.setCursor(7,1);
554        lcd.print(StearingMaxMove);
555        lcd.write(0xDF);
556       break;
557      case 5: //coefficient
558        lcd.print("coeffic.");
559        lcd.setCursor(7,1);
560        lcd.print(StearingCoeffMove);
561        lcd.print(" x ");
562        lcd.write(0xDF);
563        break;
564    }
565  }
566}
567/*
568  SerialEvent occurs whenever a new  data comes in the hardware serial RX. This
569  routine is run between each time  loop() runs, so using delay inside loop can
570  delay response. Multiple bytes  of data may be available.
571*/
572void serialEvent() {
573  while (Serial.available())  {
574    char inChar = (char)Serial.read();
575    inputString += inChar;
576    //  if the incoming character is a newline, set a flag so the main loop can
577    //  do something about it
578    if (inChar == '\n') {
579      stringComplete = true;
580      }
581    }
582  }
583
584//calculate checksum of nmea sentence
585String nmea0183_checksum(String  nmea_data) {
586    int crc = 0;
587    String chSumString = "";
588    int i;
589    // ignore the first $ sign, checksum in sentence
590    for (i = 1; i < (nmea_data.length()-5);  i ++) { // remove the - 5 if no "*" + cksum + cr + lf are present
591        crc  ^= nmea_data[i];
592    }
593    chSumString = String(crc,HEX);
594    if (chSumString.length()==1)  {
595      chSumString="0"+chSumString.substring(0,1);
596    }
597    chSumString.toUpperCase();
598    return chSumString;
599}
600
601//check RC which button is pressed
602int checkRfRC()  {
603  int n = 0;
604  int res = 0;
605  n = analogRead(rfRemoteControlPin);
606  if ((n>350) and (n<460)) { // button A
607    res = RCleftbutton;
608  }
609  if  ((n> 90) and (n<190)) { // button B
610    res = RCrightbutton;
611  }
612  if ((n>540)  and (n<640)) { // button C
613    res = RCleft10button;
614  }
615  if ((n>225)  and (n<325)) { // button D
616    res = RCright10button;
617  }
618  return res;    
619}
620
621//check HW which button is pressed
622int checkHWButtons() {
623  int n = 0;
624  int res = 0;
625  n = analogRead(ButtonsPin);  
626  //Serial.println(n);  
627  if ((n>465) and (n<565)) { // button left
628    res = HWleftbutton;
629  }
630  if ((n>290) and (n<390)) { // button right
631    res = HWrightbutton;
632  }
633  if ((n>130) and (n<220)) { // button pause
634    res = HWpausebutton;
635  }
636  if ((n>625) and (n<725)) { // button setup
637    res = HWsetupbutton;
638  }
639  if ((n>975) and (n<1075)) { // button left-10
640    res = HWleft10button;
641  }
642  if ((n>800) and (n<900)) { // button right+10
643    res = HWright10button;
644  }
645  return res;    
646}
647
648void gosetup() {  // setup button
649  if (isSetup  == false) {
650      SetupParameter = 1;
651      isSetup = true;
652    } else  {
653      if (SetupParameter < 5) {
654          SetupParameter ++;
655        }  else {
656          if (prev_StearingInterval != StearingInterval || prev_StearingMinToMove  != StearingMinToMove || prev_StearingMaxMove != StearingMaxMove || prev_StearingCoeffMove  != StearingCoeffMove) {
657            lcd.clear();
658            lcd.setCursor(0,0);
659            lcd.print("updating... ");
660            delay(1000);
661            goupdateEEPROM();
662            if (EEerr) {
663              lcd.print("E=");
664              lcd.print(EEerr);
665              delay(1000);
666            }
667            prev_StearingInterval  = StearingInterval;
668            prev_StearingMinToMove = StearingMinToMove;
669            prev_StearingMaxMove = StearingMaxMove;
670            prev_StearingCoeffMove  = StearingCoeffMove;
671          }
672          isSetup = false;
673      }
674  }
675  NewTone (speakerPin,2000);
676  delay(200);
677  noNewTone();
678  RefreshDisplay();
679}
680
681void  goupdateEEPROM() {
682  EEaddress = 0;  //id1
683  EEdata[0] = EEid1;
684  EEbytedata  = EEid1;
685  writeEEPROM (EEdisk, EEaddress, EEbytedata);
686  EEaddress = 1;  //id2
687  EEdata[1] = EEid2;
688  EEbytedata = EEid2;
689  writeEEPROM (EEdisk, EEaddress,  EEbytedata);
690  memcpy(bStearingInterval, &StearingInterval, sizeof(int));
691  memcpy(bStearingMinToMove, &StearingMinToMove, sizeof(int));
692  memcpy(bStearingMaxMove,  &StearingMaxMove, sizeof(int));
693  memcpy(bStearingCoeffMove, &StearingCoeffMove,  sizeof(float));
694  memcpy(EEdata+2,bStearingInterval,sizeof(int));
695  memcpy(EEdata+4,bStearingMinToMove,sizeof(int));
696  memcpy(EEdata+6,bStearingMaxMove,sizeof(int));
697  memcpy(EEdata+8,bStearingCoeffMove,sizeof(float));
698  for (s = 2; s < EEbytes; s ++) {
699    EEaddress = s;  //data
700    EEbytedata  = EEdata[s];
701    writeEEPROM (EEdisk, EEaddress, EEbytedata);
702  }
703}
704
705void  goleft() {  // left button/RC
706  if (ispause == false) {
707    nmf_routetofollow  --;
708    if (nmf_routetofollow < 1) {
709      nmf_routetofollow = 360;
710    }
711    d=nmf_routetofollow;
712    nmf_routetofollow=d;
713    nm_routetofollow=d;
714    NewTone (speakerPin,400);
715    delay(200);
716    noNewTone();
717  } else  {
718    NewTone (speakerPin,1000);
719    delay(50);
720    noNewTone();
721  }
722  RefreshDisplay();
723}
724
725void goleft10() {  // left 10x button/RC
726  if  (ispause == false) {
727    for (s = 1; s < 11; s ++) {
728      nmf_routetofollow  --;
729      if (nmf_routetofollow < 1) {
730        nmf_routetofollow = 360;
731      }
732    }
733    d=nmf_routetofollow;
734    nmf_routetofollow=d;
735    nm_routetofollow=d;
736    NewTone (speakerPin,400);
737    delay(200);
738    noNewTone();
739  } else  {
740    NewTone (speakerPin,1000);
741    delay(50);
742    noNewTone();
743  }
744  RefreshDisplay();
745}
746
747void goright() {  // right button/RC
748  if (ispause  == false) {
749    nmf_routetofollow ++;
750    if (nmf_routetofollow > 360) {
751      nmf_routetofollow = 1;
752    }
753    d=nmf_routetofollow;
754    nmf_routetofollow=d;
755    nm_routetofollow=d;
756    NewTone (speakerPin,800);
757    delay(200);
758    noNewTone();
759  } else {
760    NewTone (speakerPin,1000);
761    delay(50);
762    noNewTone();
763  }
764  RefreshDisplay();
765}
766
767void goright10() {  //  right 10x button/RC
768  if (ispause == false) {
769    for (s = 1; s < 11; s ++)  {
770      nmf_routetofollow ++;
771      if (nmf_routetofollow > 360) {
772        nmf_routetofollow  = 1;
773      }
774    }
775    d=nmf_routetofollow;
776    nmf_routetofollow=d;
777    nm_routetofollow=d;
778    NewTone (speakerPin,800);
779    delay(200);
780    noNewTone();
781  } else {
782    NewTone (speakerPin,1000);
783    delay(50);
784    noNewTone();
785  }
786  RefreshDisplay();
787}
788
789void gopause() {  //  pause button/RC
790  if (ispause == true) {
791    ispause=false; 
792    digitalWrite(ledpausePin,  HIGH);
793    NewTone (speakerPin,50);
794    delay(200);
795    NewTone (speakerPin,200);
796    delay(800);
797    noNewTone();
798  } else {
799    ispause=true; 
800    digitalWrite(ledpausePin,  LOW);
801    NewTone (speakerPin,200);
802    delay(200);
803    NewTone (speakerPin,50);
804    delay(800);
805    noNewTone();
806  }
807  RefreshDisplay();
808}
809
810//  reading eeprom
811byte readEEPROM (int diskaddress, unsigned int memaddress) {
812  byte rdata = 0x00;
813  Wire.beginTransmission (diskaddress);
814  Wire.write  (memaddress);
815  if (Wire.endTransmission () == 0) {
816    Wire.requestFrom (diskaddress,1);
817    if (Wire.available()) {
818        rdata = Wire.read();
819      } else {
820        EEerr = 1; //"READ no data available"
821    }
822    } else {
823      EEerr  = 2; //"READ eTX error"
824  }
825  Wire.endTransmission (true);
826  return rdata;
827}
828
829//  writing eeprom
830void writeEEPROM (int diskaddress, unsigned int memaddress, byte  bytedata) {
831  Wire.beginTransmission (diskaddress);
832  Wire.write (memaddress);
833  Wire.write (bytedata);
834  if (Wire.endTransmission () != 0) {
835    EEerr =  3; //"WRITING eTX error"
836  }
837  Wire.endTransmission (true);
838  delay(5);  
839}
840
841// round zero decimal
842float roundZeroDec(float f) {
843  float y,  d;
844  y = f*1;
845  d = y - (int)y;
846  y = (float)(int)(f*1)/1;
847  if (d >=  0.5) {
848    y += 1;
849   } else {
850    if (d < -0.5) {
851    y -= 1;
852  }
853  }
854  return y;
855}
856
857// round one decimal
858float roundOneDec(float f)  {
859  float y, d;
860  y = f*10;
861  d = y - (int)y;
862  y = (float)(int)(f*10)/10;
863  if (d >= 0.5) {
864    y += 0.1;
865   } else {
866    if (d < -0.5) {
867    y  -= 0.1;
868  }
869  }
870  return y;
871}
872
873// round two decimal
874float roundTwoDec(float  f) {
875  float y, d;
876  y = f*100;
877  d = y - (int)y;
878  y = (float)(int)(f*100)/100;
879  if (d >= 0.5) {
880    y += 0.01;
881   } else {
882    if (d < -0.5) {
883    y  -= 0.01;
884  }
885  }
886  return y;
887}
888