Linefollower-problem z kodem dla czujnika ultradźwiekowego wykrywającego przeszkody

[Jatek1]

Witam,

Zbudowałem linefollowera opierając się na tym poradniku: https://www.electronicshub.org/arduino-line-follower-robot/

Linefollower sam w sobie działa, lecz chciałem dodać możliwosć wykrywania przeszkód przed robotem.

Pomysł jest taki, aby robot po wykryciu przeszkody zatrzymał się, a gdy przeszkoda zostanie usunięta kontynuował jazdę po linii.

Do wykrywania przeszkód użyłem czujnika HC-SR04.

Mam coś takiego, lecz nie działa ten kod tak jak napisałem wyżej.

Proszę o pomoc.

Pozdrawiam

int mot1=9;
int mot2=6;
int mot3=5;
int mot4=3;

int Trig = 4;  
int Echo = 2;  
     
int left=13;
int right=12;
int Left=0;
int Right=0;
void LEFT (void);
void RIGHT (void);
void STOP (void);

void setup()
{
  pinMode(mot1,OUTPUT);
  pinMode(mot2,OUTPUT);
  pinMode(mot3,OUTPUT);
  pinMode(mot4,OUTPUT);

  pinMode(left,INPUT);
  pinMode(right,INPUT);

  digitalWrite(left,HIGH);
  digitalWrite(right,HIGH);

  pinMode(Trig, OUTPUT);                    
  pinMode(Echo, INPUT);                      
  
}

void loop() {
  
zakres(0, 25);
delay(100);

while(1)
{
  Left=digitalRead(left);
  Right=digitalRead(right);
  
  if((Left==0 && Right==1)==1)
  LEFT();
  else if((Right==0 && Left==1)==1)
  RIGHT();
}
}

int zmierzOdleglosc() {
  long czas, dystans;
 
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
 
  czas = pulseIn(Echo, HIGH);
  dystans = czas / 58;
 
  return dystans;
}
 
void zakres(int a, int b) {
  int jakDaleko = zmierzOdleglosc();
  if ((jakDaleko > a) && (jakDaleko < b)) {
      analogWrite(mot1,0);
      analogWrite(mot2,0);
      analogWrite(mot3,0);
      analogWrite(mot4,0);
      delay(10000);
      
  } else {
      analogWrite(mot1,255);
analogWrite(mot2,0);
analogWrite(mot3,255);
analogWrite(mot4,0);
  }
}
 
void LEFT (void)
{
   analogWrite(mot3,0);
   analogWrite(mot4,30);
   
   
   while(Left==0)
   {
    Left=digitalRead(left);
    Right=digitalRead(right);
    if(Right==0)
    {
      int lprev=Left;
      int rprev=Right;
      STOP();
      while(((lprev==Left)&&(rprev==Right))==1)
      {
         Left=digitalRead(left);
         Right=digitalRead(right);
      }
    }
    analogWrite(mot1,255);
    analogWrite(mot2,0); 
   }
   analogWrite(mot3,255);
   analogWrite(mot4,0);
}

void RIGHT (void)
{
   analogWrite(mot1,0);
   analogWrite(mot2,30);

   while(Right==0)
   {
    Left=digitalRead(left);
    Right=digitalRead(right);
    if(Left==0)
    {
      int lprev=Left;
      int rprev=Right;
     STOP();
      while(((lprev==Left)&&(rprev==Right))==1)
      {
         Left=digitalRead(left);
         Right=digitalRead(right);
      }
    }
    analogWrite(mot3,255);
    analogWrite(mot4,0);
    }
   analogWrite(mot1,255);
   analogWrite(mot2,0);
}

void STOP (void)
{
analogWrite(mot1,0);
analogWrite(mot2,0);
analogWrite(mot3,0);
analogWrite(mot4,0);
  
}