0

I have a problem with linefollower which detects line with infrared sensors and follows it, but I would like it to detect obstacles with an ultrasonic sensor. I would like the robot to stop after detecting an obstacle, and when it is removed, it continued along the line. What am I doing wrong in the code that the robot does not stop after detecting an obstacle, but continues on the line?

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);

}
4
  • I see a function for doing the distance measurement, but I don't see you using it anywhere – chrisl May 16 at 12:29
  • 1
    can you help me, how to write a function so that the robot stops after detecting an obstacle and continues along the line after removing it? – NEADERS May 16 at 12:37
  • Insert a while loop into your main while(1) loop, where you call the measurement function and only exit from the loop, if the measured distance is above a certain threshold. – chrisl May 16 at 13:14
  • it makes no sense to use while(1) inside loop() .... move the four analogWrite statements to the setup() function because they execute only once at startup ... then delete the while statement – jsotola May 16 at 15:45

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.