Arduino.ru

  while (flag) {
    GPSStatys(); // NEO-6m ackData[1] - GPS курс, ackData[2] - дистанция, ackData[3] - азимут, ackData[5] - КМ/ч
    if (millis() - timeoutAV >= 50) {
      timeoutAV = millis();
      target_on = false;
      StatCompass(); // Цыфровий компас ackData[4] - курс
    }
    NRF24L01();
    if (recieved_data[3] > 10) {
      flag = false;
      motorspeed = 28;
    }
    if (ackData[2] > 5) {
      servo2.write(map(180, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
      servo3.write(map(0, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
    }
    if (ackData[2] < 2) {
      if (is_one2) {
        flag = false;
        motorspeed = 28;
        is_one2 = false;
      }
      if (recieved_data[5] == 1) {
        DISTANCE_LAT = eeprom_read_float(0);
        DISTANCE_LNG = eeprom_read_float(4);
        if (is_one3) {
          for (int twoi = 180; twoi > 30; twoi--) {
            servo2.write(map(twoi, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
          }
          for (int threei = 0; threei < 150; threei++) {
            servo3.write(map(threei, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
          }
        }
        target_on = true;
        if (GPS_ON) {
          is_one2 = true;
          GPS_ON = false;
        }
      } else {
        is_one2 = true;
      }
    } else { // вычисление угола поворота на азимут
      if ( ackData[4] > ackData[3] + 40 || ackData[4] < ackData[3] - 40) {
        limitspeed = 50;
        SpeedMotor();
        if ( ackData[4] > ackData[3] ) { /// отклонение большое + - 40
          if (is_one0) {
            turn = 80;
            is_one0 = false;
          }
          is_one1 = true;
          turn++;
          if (turn >= 100) {
            turn = 100;
          }
          motor.write(map(motorspeed, 0, 255, 800, 2550));
          servo1.write(map(turn, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
        } else {
          if (is_one1) {
            turn = 80;
            is_one1 = false;
          }
          is_one0 = true;
          turn--;
          if (turn <= 60) {
            turn = 60;
          }
          motor.write(map(motorspeed, 0, 255, 800, 2550));
          servo1.write(map(turn, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
        }
      } else if ( ackData[4] > ackData[3] + 20 || ackData[4] < ackData[3] - 20) { /// отклонение средние + - 20
        limitspeed = 60;
        SpeedMotor();
        if ( ackData[4] > ackData[3] ) {
          if (is_one0) {
            turn = 80;
            is_one0 = false;
          }
          is_one1 = true;
          turn++;
          if (turn >= 90) {
            turn = 90;
          }
          motor.write(map(motorspeed, 0, 255, 800, 2550));
          servo1.write(map(turn, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
        } else {
          if (is_one1) {
            turn = 80;
            is_one1 = false;
          }
          is_one0 = true;
          turn--;
          if (turn <= 70) {
            turn = 70;
          }
          motor.write(map(motorspeed, 0, 255, 800, 2550));
          servo1.write(map(turn, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
        }
      } else { /// отклонение малое + - 5
        limitspeed = 70;
        SpeedMotor();
        if ( ackData[4] > ackData[3] ) {
          if (is_one0) {
            turn = 80;
            is_one0 = false;
          }
          is_one1 = true;
          turn++;
          if (turn >= 85) {
            turn = 85;
          }
          motor.write(map(motorspeed, 0, 255, 800, 2550));
          servo1.write(map(turn, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
        } else {
          if (is_one1) {
            turn = 80;
            is_one1 = false;
          }
          is_one0 = true;
          turn--;
          if (turn <= 75) {
            turn = 75;
          }
          motor.write(map(motorspeed, 0, 255, 800, 2550));
          servo1.write(map(turn, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH));
        }
      }
    }
  }

На это хватило ума.. 

Поворот сервы ограничено 60-100 град. Нужно чтобы servo1(правильней) выравнивало угол поворота на азимут также в этом примере есть недостаток когда азимут 359-0 то кораблик кружиться на месте в одну сторону потому-что на практике азимут меняется 350-10 к примеру, и кораблик делает полный разворот. Как пофиксить незнаю.. есть у кого какие предложение ?