Линейный следователь обнаруживает препятствия

У меня есть проблема с linefollower, который обнаруживает линию с помощью инфракрасных датчиков и следует за ней, но я бы хотел, чтобы он обнаруживал препятствия с помощью ультразвукового датчика. Мне бы хотелось, чтобы робот остановился после обнаружения препятствия, а когда оно будет устранено, то продолжил движение вдоль линии. Что я делаю не так в коде, что робот не останавливается после обнаружения препятствия, а продолжает движение по линии?

int mot1=9;
int mot2=6;
int mot3=5;
int mot4=3;
int Trig = 4;  
int Echo = 2;  
int CM;        
long CZAS;     
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() 
{

analogWrite(mot1,255);
analogWrite(mot2,0);
analogWrite(mot3,255);
analogWrite(mot4,0);

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

void pomiar_odleglosci ()
{
 digitalWrite(Trig, LOW);       
 delayMicroseconds(2);
 digitalWrite(Trig, HIGH);       
 delayMicroseconds(15);
 digitalWrite(Trig, LOW);
 digitalWrite(Echo, HIGH); 
 CZAS = pulseIn(Echo, HIGH);
 CM = CZAS / 58;  
 if(CM < 25)
 {
 analogWrite(mot1,0);
analogWrite(mot2,0);
analogWrite(mot3,0);
analogWrite(mot4,0);
}

else{
}
}
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);

}