не работает GPS-модуль GY-NEO-6M SMA micro USB Arduino

У нас модуль GY-NEO-6M SMA micro USB Arduino, который работает если загружать в него этот код:

#include <TinyGPS++.h>
#include <SoftwareSerial.h>

SoftwareSerial gpsSerial(4, 5); // RX, TX
TinyGPSPlus gps;

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

void loop() {
  while (gpsSerial.available() > 0) {
    if (gps.encode(gpsSerial.read())) {
      if (gps.location.isValid()) {
        Serial.print("Широта: ");
        Serial.println(gps.location.lat(), 6);
        Serial.print("Долгота: ");
        Serial.println(gps.location.lng(), 6);
      } else {
        Serial.println("Ожидание фиксации GPS...");
      }
    }
  }
}

Но если я обобщаю в один код для нашего проекта (он выводит данные в COM-порт, чтобы прога составляла карту айсбергов), то он прекращает работать.

Вот обобщённый код:

#include <NewPing.h>
#include <Servo.h>
#include <TinyGPS++.h>
#include <SoftwareSerial.h>

// Настройка ультразвукового датчика (SONAR)
#define TRIGGER_PIN 11
#define ECHO_PIN 12
#define MAX_DISTANCE 200  // Максимальное измеряемое расстояние (см)
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// Настройка сервомотора (рулевое управление)
#define SERVO_PIN 3
Servo steering_servo;
int current_angle = 90;  // Текущий угол руля (90 = прямо)

// Настройка GPS
static const int RXPin = 4, TXPin = 5;  // GPS модуль подключен к пинам 4 и 5
SoftwareSerial gpsSerial(RXPin, TXPin);
TinyGPSPlus gps;

// Переменные для фильтрации расстояния
float filtered_distance = 0;
float raw_distance = 0;

void setup() {
  Serial.begin(9600);      // Инициализация последовательного порта для вывода
  gpsSerial.begin(9600);   // Инициализация GPS
  steering_servo.attach(SERVO_PIN);  // Инициализация сервомотора
  steering_servo.write(current_angle);  // Установка руля в прямое положение
  
  // Вывод заголовка CSV
  Serial.println("LAT,LON,ANGLE,DISTANCE"); 
}

// Функция получения отфильтрованного расстояния
int get_filtered_distance() {
  raw_distance = sonar.ping_median(5) / 2 * 0.0343;  // 5 измерений, преобразование в см
  
  // Простой фильтр для сглаживания значений
  if (abs(raw_distance - filtered_distance) > 60) {
    // При резком изменении - сильное сглаживание
    filtered_distance = filtered_distance * 0.98 + raw_distance * 0.02;
  } else {
    // При плавном изменении - слабое сглаживание
    filtered_distance = filtered_distance * 0.6 + raw_distance * 0.4;
  }
  
  return (int)filtered_distance;
}

// Функция обработки GPS данных
void read_gps(float &lat, float &lon) {
  lat = 0;
  lon = 0;
  
  while (gpsSerial.available() > 0) {
    if (gps.encode(gpsSerial.read())) {
      if (gps.location.isValid()) {
        lat = gps.location.lat();
        lon = gps.location.lng();
      }
    }
  }
}

// Функция принятия решения об объезде
void avoid_iceberg(int distance) {
  if (distance < 150) {          // Если айсберг ближе 1.5 метра
    if (current_angle == 90) {   // Если ехали прямо
      current_angle = 45;        // Поворачиваем на 45° влево
    } else {
      current_angle = 90;       // Иначе поворачиваем вправо
    }
    steering_servo.write(current_angle);
    delay(500);  // Даем время на поворот
  } else {
    current_angle = 90;  // Двигаемся прямо
    steering_servo.write(current_angle);
  }
}

void loop() {
  // 1. Получаем GPS координаты
  float latitude, longitude;
  read_gps(latitude, longitude);
  
  // 2. Измеряем расстояние до айсберга
  int distance_to_iceberg = get_filtered_distance();
  
  // 3. Принимаем решение о направлении движения
  avoid_iceberg(distance_to_iceberg);
  
  // 4. Выводим данные в заданном формате
  Serial.print(latitude, 6);
  Serial.print(",");
  Serial.print(longitude, 6);
  Serial.print(",");
  Serial.print(current_angle);
  Serial.print(",");
  Serial.println(distance_to_iceberg);
  
  delay(100);  // Небольшая задержка между измерениями
}