Проблема с датчиком AJ-SR04M

Программа передаёт данные в COM (координаты (gps не работает), угол корабля, расстояние до айсберга). Пока программа не дописана, т. к. сейчас пишу именно расстояние. Сейчас проблема в том, что какое бы расстояние не передавала программа (0 зачастую), всё равно поворачивается серво-мотор. У меня ультразвуковой датчик AJ-SR04M и мы делаем на Arduino Mega, серво у меня стандартный - ардуиновско-наборный)), вот код который я сделала, где всё работает хорошо

#include <NewPing.h>

#define TRIGGER_PIN  11
#define ECHO_PIN     12
#define MAX_DISTANCE 400
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
float tempval1;
float tempval2;
int finalval;

void setup() {
  Serial.begin(9600);
}

void loop() {
  delay(1000);                    
  Serial.print("Ping: ");
  int iterations = 5;
  tempval1=((sonar.ping_median(iterations) / 2) * 0.0343);
  if(tempval1-tempval2>60 || tempval1-tempval2<-60)
  {
    tempval2=(tempval1*0.02 )+ (tempval2*0.98);
    }
  else
  {
  tempval2=(tempval1*0.4 )+ (tempval2*0.6);
  }
  finalval=tempval2;
  Serial.print(finalval);
  Serial.println("cm");
}

Вот проект

Вот полный код программы

#include <NewPing.h>
#include <Servo.h>

// Пины для AJ-SR04M
#define TRIGGER_PIN 11
#define ECHO_PIN 12
#define MAX_DISTANCE 400
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
float tempval1;
float tempval2;
int finalval;

// Исправлено: возвращаемый тип изменён с void на float
float get_distance() {
  delay(1000);                    
  Serial.print("Ping: ");
  int iterations = 5;
  tempval1=((sonar.ping_median(iterations) / 2) * 0.0343);
  if(tempval1-tempval2>60 || tempval1-tempval2<-60)
  {
    tempval2=(tempval1*0.02 )+ (tempval2*0.98);
    }
  else
  {
  tempval2=(tempval1*0.4 )+ (tempval2*0.6);
  }
  finalval=tempval2;
  Serial.print(finalval);
  Serial.println("cm");
  return int(finalval);
}

// Пин для сервомотора (руль)
#define SERVO_PIN 9

// Пины для моторов

Servo servo;
int check_angles[] = {45, 90, 135};

void setup() {
  Serial.begin(9600);
  Serial.println("Starting Iceberg Avoider...");

  // Настройка сервомотора
  servo.attach(SERVO_PIN);
  servo.write(90);
  Serial.println("Servo set to 90 degrees");
}

void loop() {
  static float lat = 59.35;
  static float lon = 40.35;
  static int history[5];
  static int history_index = 0;
  static float distances[3];

  // Проверка расстояний
  for (int i = 0; i < 3; i++) {
    servo.write(check_angles[i]);
    Serial.print("Servo angle: ");
    Serial.println(check_angles[i]);
    delay(50);
    distances[i] = get_distance();
    
    // Отправка данных
    Serial.print(lat, 2);
    Serial.print(",");
    Serial.print(lon, 2);
    Serial.print(",");
    Serial.print(check_angles[i]);
    Serial.print(",");
    Serial.println(distances[i], 2);
  }

  // Обновление координат
  lat += 0.01;
  lon += 0.01;

  // Объезд
  int best_angle = find_best_direction(distances, history, &history_index);
  Serial.print("Best angle: ");
  Serial.println(best_angle);

  if (distances[1] < 2 && distances[1] >= 1) {
    
    servo.write(best_angle);
    Serial.println("Obstacle < 2cm, turning");
    delay(50);
  } else {
    servo.write(90);
    Serial.println("Moving forward");
  }

  delay(50);
}

int find_best_direction(float distances[], int history[], int *history_index) {
  int best_angle = 90;
  float max_distance = distances[1];

  for (int i = 0; i < 3; i++) {
    if (distances[i] > max_distance) {
      max_distance = distances[i];
      best_angle = check_angles[i];
    }
  }

  int recent_count = 0;
  for (int i = 0; i < 5; i++) {
    if (history[i] == best_angle) recent_count++;
  }
  if (recent_count > 2) {
    float second_max = 0;
    for (int i = 0; i < 3; i++) {
      if (distances[i] > second_max && check_angles[i] != best_angle) {
        second_max = distances[i];
        best_angle = check_angles[i];
      }
    }
  }

  history[*history_index] = best_angle;
  *history_index = (*history_index + 1) % 5;
  return best_angle;
}