Złe działanie czujnika odległości HC-SR04 na serwie - pomiary 0 cm

[Poczatkujacy]

Witam,
Jestem w 8 klasie (co pewnie da się wyczuć 🙂 ) i robię robota, który omijałby przeszkody i wracał na trasę z wykorzystaniem HC-SR04 i MPU-6050. Używam też shielda od forbota, omawianego w kursie robotyki.
Nieruchomy czujnik działa prawidłowo, ale kiedy obraca się na serwie, w pozycjach około prostopadłych do tylnej osi daje odczyt 0 cm.
Pozostałe odczyty są poprawne, samo podłączenie czujnika też jest w porządku, bo działa bardzo dobrze będąc nieruchomym. Założenie jest takie, żeby czujnik zamontowany na serwie  wykonywał pomiary odległości wokół robota, a następnie aby serwo obróciło się tam, gdzie odległość była najmniejsza (to pozwala cały czas monitorować najbliższą przeszkodę).
Cały ,,układ" jest bardzo prosty, w zasadzie można go zreplikować w 5 minut i potestować z moim kodem. Poza tym, zamieszczony plik .zip to minutowy film, który pokazuje zarówno ruchy serwa, jak i odczyty podawane na port szeregowy (Forobot nie pozwala na przesyłanie .mp4).
 Cały kod zdaje się działać dobrze, jedynym problemem jest to, że pomiary bywają zerowe, co sprawia, że czujnik zachowuje się bez sensu. Myślałem nad dodaniem prostego warunku:
,,Jeżeli zmierzona odległość byłaby mniejsza od minimalnej i jednocześnie większa od zera, wtedy zapisujemy ją jako nową minimalną wartość".
W moim kodzie wyglądałoby to tak:
if (odleglosci[x] < min_odleglosc && odleglosci[x] > 0)
min_odleglosc = odleglosci[x];
Niestety, w tym przypadku robot zupełnie nie wykrywałby przeszkód bezpośrednio naprzeciwko niego, bo otrzymywałby tam pomiary 0 cm (rysunek w załącznikach).
Czy ktoś wie, jak naprawić kod/elektronikę?
Z góry dziękuję za pomoc.
Połączenia czujnik - Arduino:
Vcc - 5V
Gnd - Gnd
trig - D8
echo - D7

//w przod - (R_DIR, 1), (L_DIR, 0). Min. predkosc silnikow to 20 w funkcji Left- i Rightmotor. RightMotor - słabszy
//w accelerate dodac zczytywanie obecnej v i porownywac z poprzednia v, plynnie zwalniac
//tak naprawde wystarcza dwie zmienne: aktualna_odleglosc i max_odleglosc. Jezeli aktualna_odleglosc > max_odleglosc, staje sie max_odleglosc. Jesli nie, w ogole jej nie potrzebujemy
#define L_PWM 5
#define L_DIR 4
#define R_PWM 6
#define R_DIR 9
#define PWM_MAX 165
#define v_min 20
#define trig 8
#define echo 7
#define SERWO_PIN 11
#include <Servo.h>
Servo serwo; 
float odleglosc;
float LeftMotorMargin = 5;
int max_odleglosc_global = 0;
int min_odleglosc_global = 100;
int min_odleglosc = 100;
int max_odleglosc = 0;
int odleglosci [20];
int powtorzenia = 0;
int v_previous = 0;
int j, x;
int do_usuniecia = 0;
int poprzedni_kat_serwa;
int czestotliwosc_obrotu_serwa = 18;
int int_numer_pomiaru;
void LeftMotor (int v)//funkcja sterująca silnikiem lewym z jednym argumentem, prędkością.
{
  if (v>0) //jeżeli podana wartość jest dodatnia
  {
  v+=LeftMotorMargin;
  v = map(v,0,100,0,PWM_MAX);//zmieniamy wartość podaną w programie na PWM
  digitalWrite (L_DIR, 0);//wyznaczamy kierunek - do przodu
  analogWrite (L_PWM, v);//PWM równy temu, co wyznaczył uzytkownik
  } else//jeżeli podana liczba jest ujemna
  {
   v = abs(v);//zamieniamy liczbę na dodatnią
   v+=LeftMotorMargin;
   //v = v - LeftMotorMargin;
   v = map(v,0,100,0,PWM_MAX);//zmieniamy podaną wartość na PWM
   digitalWrite (L_DIR, 1);//wyznaczamy kierunek - do tyłu
   analogWrite (L_PWM, v);//PWM równy temu, co wyznaczył użytkownik
  }
}
void RightMotor (int v)//analogiocznie do lewego silnika
{
  if (v>0)
  {
  
  v = map(v,0,100,0,PWM_MAX);
  digitalWrite (R_DIR, 1);
  analogWrite (R_PWM, v);
  } else
  {
   v = abs(v);
   
   v = map(v,0,100,0,PWM_MAX);
   digitalWrite (R_DIR, 0);
   analogWrite (R_PWM, v);
  }
}
void stopMotors()//zatrzymanie silników
{
  analogWrite(L_PWM, 0);
  analogWrite(R_PWM,0);
}
void accelerate (int v)//funkcja na płynne przyspieszanie i zwalnianie
{
  if (abs(v)<20)//jeżeli podana liczba jest mniejsza niż 20
  {
    v=20; //prędkość równa się 20 (unikamy prędkości zbyt małych, by ruszyć).
  }
  
  Serial.println (v); 
  if (v>v_previous)//jeżeli przyspieszamy
  {
    for (int i = v_previous; i<v; i++)//zacznij od poprzedniej wartości, zwiększaj do wyznaczonej do wyznaczonej
    {
     RightMotor(i);//do przodu
     LeftMotor(i);
     delay(50); 
    }
  }
  if (v<v_previous)//jeżeli zwalniamy
  {
    for (int i=v_previous; i>v; i--)//zacznij od wartości poprzedniej, zmniejszaj do wyznaczonej 
    {
      RightMotor(i);
      LeftMotor(i);
      delay(50);
    }
  }
  v_previous = v;//zapisz obecną wartość jako wyznaczoną
}
float pomiar_odleglosci ()
{
  float odleglosc, czas;
  digitalWrite (trig, LOW);
  delayMicroseconds (2);
  digitalWrite (trig, HIGH);
  delayMicroseconds (10);
  digitalWrite (trig, LOW);
  czas = pulseIn (echo, HIGH);
  odleglosc = czas / 58;
  return odleglosc;
}
void serwo_right ()
{
  min_odleglosc = 100;
  for (int i = poprzedni_kat_serwa; i>0; i--)
  {
    serwo.write(i); 
    delay(10); //czujnik wychyla sie w prawo, resetujemy min_odleglosc
}
poprzedni_kat_serwa = serwo.read();
}
void serwo_left ()
{ 
  odleglosci[x] = pomiar_odleglosci ();
  Serial.println ("odleglosc prawa");
  Serial.println (x);
  Serial.println (odleglosci[x]);
  x++;//wykonujemy pierwszy pomiar (w innym wypadku miałby on miejsce dopiero po wychyleniu serwa o czestotliwosc_obrotu_serwa, czyli 18 stopni)
  for (int i = poprzedni_kat_serwa; i<=180; i++)
  { //posuwamy sie az do skrajnie lewego wychylenia (zaczynamy ze skrajnie prawego)
    if (x>180/czestotliwosc_obrotu_serwa)
    x=0; //jezeli wykonalismy pelen cykl pomiarow resetujemy ich licznik
    if (j>=czestotliwosc_obrotu_serwa)//kiedy czujnik skrecil o kat podany jako czestotliwosc_pomiaru (kiedy trzeba wykonac pomiar)
    {
      
      j=0; //resestujemy licznik umozliwiajacy wykonanie pomiaru co czestotliwosc_pomiaru
      odleglosci[x] = pomiar_odleglosci();//wykonujemy pomiar
      if (odleglosci[x]>max_odleglosc)//jezeli aktualna odleglosc jest wieksza od zapisanej jako najwieksza odleglosc, staje sie nowa najwieksza odlegloscia
      max_odleglosc = odleglosci[x];
      if (odleglosci[x]<min_odleglosc)//jezeli aktualna odleglosc jest mniejsza od zapisanej jako najmniejsza odleglosc
      min_odleglosc = odleglosci[x];
      if (i < 90)//kiedy czujnik jest wychylony w lewo
      {
      Serial.println ("odleglosc prawa");//wypisujemy aktualna odleglosc
      Serial.println (x);
      Serial.println (odleglosci[x]);
      }else //kiedy czujnik jest wychylony w prawo
      {
      Serial.println ("odleglosc lewa"); //wypisujemy aktualna odleglosc
      Serial.println (x);
      Serial.println (odleglosci[x]);
      }
      x++; //zwiekszamy licznik pomiarow
      }
    //Serial.print ("Kat serwa = "); opcjonalnie mozna odkomentowac, pozwala zobaczyc kat serwa
    //Serial.println (serwo.read());   
    serwo.write(i);//przesuwamy serwo 
    j++;//zmieniamy licznik przesuniec, dzieki temu mozemy robic pomiary kiedy osiognie wartosc = czestotliwosc_pomiaru
    delay(10);
    }
    poprzedni_kat_serwa = serwo.read();//zapisujemy obecny kat obrotu jako poprzedni_kat_serwo, co pomaga w wykonywaniu innych obrotow

  }
  
void serwo_mid ()//kierujemy serwo do pozycji prostopadlej do tylnej do osi
{
  if (poprzedni_kat_serwa > 90)//jezeli serwo jest zwrocone w lewo
  {
    for (int i = poprzedni_kat_serwa; i>90; i--)//zmniejszamy, czyli idziemy do prawej
    {
      serwo.write(i);
    }
  }else //kiedy serwo jest zwrocone w lewo
  {
   for (int i = poprzedni_kat_serwa; i<90; i++)//zwiekszamy, czyli idziemy do lewej
    {
      serwo.write(i);
    } 
  }
  poprzedni_kat_serwa = serwo.read();//zapisujemy obecny kat obrotu jako poprzedni_kat_serwo, co pomaga w wykonywaniu innych obrotow
}
void zadany_obrot (int kat)//obrot serwa do zadanego katu
{
   if (poprzedni_kat_serwa > kat)//jezeli zadany kat jest na prawo od obecnego
  {
    for (int i = poprzedni_kat_serwa; i>kat; i--)//skrecamy serwo w prawo
    {
      serwo.write(i);
    }
  }else //jezeli zadany kat jest na lewo od obecnego
  {
   for (int i = poprzedni_kat_serwa; i<kat; i++)//skrecamy serwo w lewo
    {
      serwo.write(i);
    } 
  }
  poprzedni_kat_serwa = serwo.read();//zapisujemy obecny kat obrotu jako poprzedni_kat_serwo, co pomaga w wykonywaniu innych obrotow 
}
int numer_pomiaru ()//zczytujemy tablice odleglosci[] (wyniki pomiarow) i szukamy tego, ktory jest najmniejszy
{
 for (int i = 0; i <180/czestotliwosc_obrotu_serwa; i++)//przeszukujemy wszystkie pomiary
{
  if (odleglosci[i] == min_odleglosc)//jezeli odleglosc o numerze 'i' jest najmniejsza
  {
   return i;//zwracamy numer pomiaru
  }
  
} 
}
float pomiar_odleglosci_serwo()//uzywamy trzech wyzej opisanych funkcji do zrobienia pomiaru odleglosci
{
  
  serwo_right();
  serwo_left();
  serwo_mid();
  return min_odleglosc;
}
void setup() {
  //definicja pinow, start komunikacji
  pinMode(L_DIR, OUTPUT);
  pinMode(R_DIR, OUTPUT);
  pinMode(L_PWM, OUTPUT);
  pinMode(R_PWM, OUTPUT);
  pinMode (trig, OUTPUT);
  pinMode (echo, INPUT);
  serwo.attach(SERWO_PIN);
  Serial.begin (9600);
  serwo_mid();
  
}
void loop() 
{
  //wypisywanie wynikow pomiarow w porcie szeregowym
 Serial.println ("Pomiar odleglosci");
 min_odleglosc_global = pomiar_odleglosci_serwo();//sprawdzamy odleglosci od przeszkod wokol robota
 delay(1000);
 int_numer_pomiaru = numer_pomiaru();//znajdujemy najmniejszy pomiar
 Serial.println ("numer pomiaru");
 Serial.println (int_numer_pomiaru);
 Serial.println ("min_odleglosc");
 Serial.println (min_odleglosc_global);
 delay(1000);
 Serial.println ("zadany obrot");
 zadany_obrot(int_numer_pomiaru*czestotliwosc_obrotu_serwa);//obracamy serwo do kata, na ktorym byl najmniejszy pomiar
 Serial.println ("kat");
 Serial.println (int_numer_pomiaru*czestotliwosc_obrotu_serwa);
}

 

film z dzialania robota.zip