grid following object height detection robot

m using arduino mega to make a grid following robot to check the heights of 3 blocks placed at any node…my bot works fine uptil coordinate(3,6) and then line follows without detecting any node henceforthif it encounters 1 or 2 blocks in the mean time…if it does not encounter any block till then then it works till (3,3) and then has the same problem…pls help!

void grid_check()
{
  if (
      (!digitalRead(ELs)) && (!digitalRead(ERs)) && (!digitalRead(Ls)) && (!digitalRead(Ms)) && (!digitalRead(Rs))     //if at grid
      )
     
  {
    MotorControl(0,0); 
    lcd.clear(); lcd.setCursor(0,0); lcd.print(x); lcd.print(y);      //print coods
    
    if(x==0 && y==0)
      {
        turn_left();
        F=forward_sensor();
        lcd.setCursor(0,1);lcd.print(F);
        if(F<23)                             //00
        {
          n++; 
          c=millis();
          while(c+700>millis()) {line_follow();}
          MotorControl(0,0);
          height_sensorforward();
          gripper_hold();
          u_turn();
          c=millis();
          while(c+500>millis()) {line_follow();}
          gripper_release();
          c=millis();
          while(c+700>millis()) {MotorControl(2,2);}
          u_turn();
        }
       else                                 //00
       {
          c=millis();
          while(c+500>millis()) {line_follow();}
       }
        goto a;
      }
      
     if(x==1 && y==0)
      { 
        turn_right();
        F=forward_sensor();
        L=left_sensor();
        lcd.setCursor(0,1);lcd.print(L);lcd.print(" ");lcd.print(F);
        if(L<23)                                                              //10
        {
          n++;
          turn_left();
          height_sensor();
          turn_right_block();
        }
        if(F<23)                             //10
        {
          n++; 
          c=millis();
          while(c+700>millis()) {line_follow();}
          MotorControl(0,0);
          height_sensorforward();
          gripper_hold();
          u_turn();
          c=millis();
          while(c+500>millis()) {line_follow();}
          gripper_release();
          c=millis();
          while(c+700>millis()) {MotorControl(2,2);}
          u_turn();
        }
        if( (L>23 && F>23) || (L<23 && F>23) )                              //10
        {
          c=millis();
          while(c+500>millis()) {line_follow();}
        }
      goto a;
      }
      
       if( (x==1 && y==1) || (x==1 && y==2) || (x==1 && y==3) || (x==1 && y==4) || (x==1 && y==5) )
      {
        L=left_sensor();
        R=right_sensor();
        F=forward_sensor();
        lcd.setCursor(0,1);lcd.print(L);lcd.print(" ");lcd.print(R);lcd.print(" ");lcd.print(F);
        if(L<23 && R>23)                                                                                        //11 12 13 14 15
        {
          n++;
          turn_left();
          height_sensor();
          turn_right_block();
        }
        if(R<23 && L>23)                                                                                         //11 12 13 14 15
        {
          n++;
          turn_right();
          height_sensor();
          turn_left_block();
        }
        if(R<23 && L<23)
        {
          n=n+2;
          turn_right();
          height_sensor();
          u_turn();
          height_sensor();
          turn_right_block();
        }
        if(F<23)                             //11 12 13 14 15
        {
          n++; 
          c=millis();
          while(c+700>millis()) {line_follow();}
          MotorControl(0,0);
          height_sensorforward();
          gripper_hold();
          u_turn();
          c=millis();
          while(c+500>millis()) {line_follow();}
          gripper_release();
          c=millis();
          while(c+700>millis()) {MotorControl(2,2);}
          u_turn();
        }
        
          c=millis();
          while(c+500>millis()) {line_follow();}
        
      goto a;
      }
      
       if(x==1 && y==6)                     
      {
        R=right_sensor();
        lcd.setCursor(0,1);lcd.print(R);
        if(R<23)                                       //16
        {
          n++;
          turn_right();
          height_sensor();
          u_turn();
        }
        if(R>23)
        {
          turn_left();
        }
        F=forward_sensor();
        lcd.setCursor(5,1);lcd.print(F);
        if(F<23)                             //16
        {
          n++; 
          c=millis();
          while(c+700>millis()) {line_follow();}
          MotorControl(0,0);
          height_sensorforward();
          gripper_hold();
          u_turn();
          c=millis();
          while(c+500>millis()) {line_follow();}
          gripper_release();
          c=millis();
          while(c+700>millis()) {MotorControl(2,2);}
          u_turn();
        }
        if(F>23)                                         //16
        {
          c=millis();
          while(c+500>millis()) {line_follow();}
        }
       goto a;
      }
      
       
     
      if(x==3 && y==6)
     {
      turn_left();
      R=right_sensor();
      F=forward_sensor();
      lcd.setCursor(0,1);lcd.print(R);lcd.print(" ");lcd.print(F);
      if(R<23)                                                                   //36
        {
          n++;
          turn_right();
          height_sensor();
          turn_left_block();
        }
      if(F<23)                             //36
        {
          n++; 
          c=millis();
          while(c+700>millis()) {line_follow();}
          MotorControl(0,0);
          height_sensorforward();
          gripper_hold();
          u_turn();
          c=millis();
          while(c+500>millis()) {line_follow();}
          gripper_release();
          c=millis();
          while(c+700>millis()) {MotorControl(2,2);}
          u_turn();
        }
       
         c=millis();
         while(c+500>millis()) {line_follow();}
       
      goto a;
     }
     
      if( (x==3 && y==5) || (x==3 && y==4) || (x==3 && y==3) || (x==3 && y==2) || (x==3 && y==1)  )
     {
       R=right_sensor();
       F=forward_sensor();
       lcd.setCursor(0,1);lcd.print(R);lcd.print(" ");lcd.print(F);
      if(R<23)                                                              //35 34 33 32 31
        {
          n++;
          turn_right();
          height_sensor();
          turn_left_block();
        }
      if(F<23)                             //35 34 33 32 31
        {
          n++; 
          c=millis();
          while(c+700>millis()) {line_follow();}
          MotorControl(0,0);
          height_sensorforward();
          gripper_hold();
          u_turn();
          c=millis();
          while(c+500>millis()) {line_follow();}
          gripper_release();
          c=millis();
          while(c+700>millis()) {MotorControl(2,2);}
          u_turn();
        }
       
         c=millis();
         while(c+500>millis()) {line_follow();}
       
      goto a;
     }
      
      if(x==3 && y==0)
     {
       R=right_sensor();
       lcd.setCursor(0,0);lcd.print(x);lcd.print(y);
       lcd.setCursor(0,1);lcd.print(R);
       if(R<23)                                            //30
        {
          n++;
          turn_right();
          height_sensor();
          turn_left_block();
        }
        MotorControl(0,0); delay(100000);                    //reduce delay in final stepup
     }
     
     
     a:
     
     if(dir=='N') y++;
     else if(dir=='S') y--;
     else if(dir=='W') x++;
     else if(dir=='E') x--;
     
  }
  
  else {line_follow();}
}
          



void loop()
{
  while(n<3)
  {
    grid_check();
  }
}

"goto"s and recursion?

Incoming!

Edit: I misread it.
Still, no “line_follow” function.

if(R<23)                                                              //35 34 33 32 31

Nice comment :wink:

if(F<23)                             //36
        {
          n++; 
          c=millis();
          while(c+700>millis()) {line_follow();}
          MotorControl(0,0);
          height_sensorforward();
          gripper_hold();
          u_turn();
          c=millis();
          while(c+500>millis()) {line_follow();}
          gripper_release();
          c=millis();
          while(c+700>millis()) {MotorControl(2,2);}
          u_turn();
        }
       
         c=millis();
         while(c+500>millis()) {line_follow();}
       
      goto a;

I have no idea what this does but it is poorly programmed.

First off, there are a few sections of code that seem almost identical to this - you should reorganize the code so there only needs to be one version of it.

Because of the possibility of millis() rollover all calculations involving it should use subtraction so instead of while (c+ 500 > millis) use while (millis() - c <= 500)

All those WHILE loops prevent the Arduino from doing other things. Look at how timing is managed in several things at a time. Your goal should be to write code that allows loop() to repeat hundreds or thousands of times per second.

…R