Zumo Robot Maze Solver Stopping At Intersection

Hello Everyone,

I am using the example for maze solver. Somehow my zumo robot will stop at intersections… Is anyone else having this problem. How can i fix this? Do i need to adjust INCHES_TO_ZUNITS? Higher or lower on ZUNITS?

#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>
#include <ZumoMotors.h>
#include <ZumoBuzzer.h>
#include <Pushbutton.h>

/* This example uses the Zumo Reflectance Sensor Array
 * to navigate a black line maze with no loops. This program
 * is based off the 3pi maze solving example which can be
 * found here:
 *
 * https://www.pololu.com/docs/0J21/8.a
 * 
 * The Zumo first calibrates the sensors to account
 * for differences of the black line on white background.
 * Calibration is accomplished in setup().
 *
 * In loop(), the function solveMaze() is called and navigates
 * the Zumo until it finds the finish line which is defined as
 * a large black area that is thick and wide enough to
 * cover all six sensors at the same time.
 * 
 * Once the Zumo reaches the finishing line, it will stop and
 * wait for the user to place the Zumo back at the starting
 * line. The Zumo can then follow the shortest path to the finish
 * line.
 *
 * The macros SPEED, TURN_SPEED, ABOVE_LINE(), and LINE_THICKNESS 
 * might need to be adjusted on a case by case basis to give better 
 * line following results.
 */

// SENSOR_THRESHOLD is a value to compare reflectance sensor
// readings to to decide if the sensor is over a black line
#define SENSOR_THRESHOLD 300

// ABOVE_LINE is a helper macro that takes returns
// 1 if the sensor is over the line and 0 if otherwise
#define ABOVE_LINE(sensor)((sensor) > SENSOR_THRESHOLD)

// Motor speed when turning. TURN_SPEED should always
// have a positive value, otherwise the Zumo will turn
// in the wrong direction.
#define TURN_SPEED 200

// Motor speed when driving straight. SPEED should always
// have a positive value, otherwise the Zumo will travel in the
// wrong direction.
#define SPEED 200 

// Thickness of your line in inches
#define LINE_THICKNESS .75 

// When the motor speed of the zumo is set by
// motors.setSpeeds(200,200), 200 is in ZUNITs/Second.
// A ZUNIT is a fictitious measurement of distance
// and only helps to approximate how far the Zumo has
// traveled. Experimentally it was observed that for 
// every inch, there were approximately 17142 ZUNITs.
// This value will differ depending on setup/battery
// life and may be adjusted accordingly. This value
// was found using a 75:1 HP Motors with batteries
// partially discharged.
#define INCHES_TO_ZUNITS 17142

// When the Zumo reaches the end of a segment it needs
// to find out three things: if it has reached the finish line,
// if there is a straight segment ahead of it, and which
// segment to take. OVERSHOOT tells the Zumo how far it needs
// to overshoot the segment to find out any of these things.
#define OVERSHOOT(line_thickness)(((INCHES_TO_ZUNITS * (line_thickness)) / SPEED))

ZumoBuzzer buzzer;
ZumoReflectanceSensorArray reflectanceSensors;
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON);

// path[] keeps a log of all the turns made
// since starting the maze
char path[100] = "";
unsigned char path_length = 0; // the length of the path

void setup()
{

  unsigned int sensors[6];
  unsigned short count = 0;
  unsigned short last_status = 0;
  int turn_direction = 1;  

  buzzer.play(">g32>>c32");
  
  reflectanceSensors.init();
  
  delay(500);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);        // turn on LED to indicate we are in calibration mode
   
  button.waitForButton();
  
  // Calibrate the Zumo by sweeping it from left to right
  for(int i = 0; i < 4; i ++)
  {
    // Zumo will turn clockwise if turn_direction = 1.
    // If turn_direction = -1 Zumo will turn counter-clockwise.
    turn_direction *= -1;
	
    // Turn direction.
    motors.setSpeeds(turn_direction * TURN_SPEED, -1*turn_direction * TURN_SPEED);
      
    // This while loop monitors line position
    // until the turn is complete. 
    while(count < 2)
    {
      reflectanceSensors.calibrate();
      reflectanceSensors.readLine(sensors);
      if(turn_direction < 0)
      {
        // If the right  most sensor changes from (over white space -> over 
        // line or over line -> over white space) add 1 to count.
        count += ABOVE_LINE(sensors[5]) ^ last_status;
        last_status = ABOVE_LINE(sensors[5]);
      }
      else
      {
        // If the left most sensor changes from (over white space -> over 
        // line or over line -> over white space) add 1 to count.
        count += ABOVE_LINE(sensors[0]) ^ last_status;
        last_status = ABOVE_LINE(sensors[0]);	  
      }
    }
  
    count = 0;
    last_status = 0;
  }
  
  // Turn left.
  turn('L');
  
  motors.setSpeeds(0, 0);
  
  // Sound off buzzer to denote Zumo is finished calibrating
  buzzer.play("L16 cdegreg4");
  
  // Turn off LED to indicate we are through with calibration
  digitalWrite(13, LOW);
}

void loop()
{

  // solveMaze() explores every segment
  // of the maze until it finds the finish
  // line.
  solveMaze();
  
  // Sound off buzzer to denote Zumo has solved the maze
  buzzer.play(">>a32");
  
  // The maze has been solved. When the user
  // places the Zumo at the starting line
  // and pushes the Zumo button, the Zumo
  // knows where the finish line is and
  // will automatically navigate.
  while(1)
  {
    button.waitForButton();
    goToFinishLine();
    // Sound off buzzer to denote Zumo is at the finish line.
    buzzer.play(">>a32");
  }
}

// Turns according to the parameter dir, which should be 
// 'L' (left), 'R' (right), 'S' (straight), or 'B' (back).
void turn(char dir)
{

  // count and last_status help
  // keep track of how much further
  // the Zumo needs to turn.
  unsigned short count = 0;
  unsigned short last_status = 0;
  unsigned int sensors[6];
  
  // dir tests for which direction to turn
  switch(dir)
  {
  
  // Since we're using the sensors to coordinate turns instead of timing them, 
  // we can treat a left turn the same as a direction reversal: they differ only 
  // in whether the zumo will turn 90 degrees or 180 degrees before seeing the 
  // line under the sensor. If 'B' is passed to the turn function when there is a
  // left turn available, then the Zumo will turn onto the left segment.
    case 'L':
	case 'B':
      // Turn left.
      motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
      
      // This while loop monitors line position
      // until the turn is complete. 
      while(count < 2)
      {
        reflectanceSensors.readLine(sensors);
		
        // Increment count whenever the state of the sensor changes 
		// (white->black and black->white) since the sensor should 
		// pass over 1 line while the robot is turning, the final 
		// count should be 2
        count += ABOVE_LINE(sensors[1]) ^ last_status; 
        last_status = ABOVE_LINE(sensors[1]);
      }
    
    break;
    
    case 'R':
      // Turn right.
      motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
      
      // This while loop monitors line position
      // until the turn is complete. 
      while(count < 2)
      {
        reflectanceSensors.readLine(sensors);
        count += ABOVE_LINE(sensors[4]) ^ last_status;
        last_status = ABOVE_LINE(sensors[4]);
      }
    
    break;
	
    case 'S':
    // Don't do anything!
    break;
  }
}

// This function decides which way to turn during the learning phase of
// maze solving.  It uses the variables found_left, found_straight, and
// found_right, which indicate whether there is an exit in each of the
// three directions, applying the "left hand on the wall" strategy.
char selectTurn(unsigned char found_left, unsigned char found_straight,
  unsigned char found_right)
{
  // Make a decision about how to turn.  The following code
  // implements a left-hand-on-the-wall strategy, where we always
  // turn as far to the left as possible.
  if(found_left)
    return 'L';
  else if(found_straight)
    return 'S';
  else if(found_right)
    return 'R';
  else
    return 'B';
}

// The maze is broken down into segments. Once the Zumo decides
// which segment to turn on, it will navigate until it finds another
// intersection. followSegment() will then return after the
// intersection is found.
void followSegment()
{
  unsigned int position;
  unsigned int sensors[6];
  int offset_from_center;
  int power_difference;
  
  while(1)
  {     
    // Get the position of the line.
    position = reflectanceSensors.readLine(sensors);
     
    // The offset_from_center should be 0 when we are on the line.
    offset_from_center = ((int)position) - 2500;
     
    // Compute the difference between the two motor power settings,
    // m1 - m2.  If this is a positive number the robot will turn
    // to the left.  If it is a negative number, the robot will
    // turn to the right, and the magnitude of the number determines
    // the sharpness of the turn.
    power_difference = offset_from_center / 3;
     
    // Compute the actual motor settings.  We never set either motor
    // to a negative value.
    if(power_difference > SPEED)
      power_difference = SPEED;
    if(power_difference < -SPEED)
      power_difference = -SPEED;
     
    if(power_difference < 0)
      motors.setSpeeds(SPEED + power_difference, SPEED);
    else
      motors.setSpeeds(SPEED, SPEED - power_difference);
     
    // We use the inner four sensors (1, 2, 3, and 4) for
    // determining whether there is a line straight ahead, and the
    // sensors 0 and 5 for detecting lines going to the left and
    // right.
     
    if(!ABOVE_LINE(sensors[0]) && !ABOVE_LINE(sensors[1]) && !ABOVE_LINE(sensors[2]) && !ABOVE_LINE(sensors[3]) && !ABOVE_LINE(sensors[4]) && !ABOVE_LINE(sensors[5]))
    {
      // There is no line visible ahead, and we didn't see any
      // intersection.  Must be a dead end.            
      return;
    }
    else if(ABOVE_LINE(sensors[0]) || ABOVE_LINE(sensors[5]))
    {
      // Found an intersection.
      return;
    }
   
  }
}

// The solveMaze() function works by applying a "left hand on the wall" strategy: 
// the robot follows a segment until it reaches an intersection, where it takes the 
// leftmost fork available to it. It records each turn it makes, and as long as the 
// maze has no loops, this strategy will eventually lead it to the finish. Afterwards, 
// the recorded path is simplified by removing dead ends. More information can be 
// found in the 3pi maze solving example.
void solveMaze()
{
    while(1)
    {
        // Navigate current line segment
        followSegment();
         
        // These variables record whether the robot has seen a line to the
        // left, straight ahead, and right, while examining the current
        // intersection.
        unsigned char found_left = 0;
        unsigned char found_straight = 0;
        unsigned char found_right = 0;
         
        // Now read the sensors and check the intersection type.
        unsigned int sensors[6];
        reflectanceSensors.readLine(sensors);
         
        // Check for left and right exits.
        if(ABOVE_LINE(sensors[0]))
            found_left = 1;
        if(ABOVE_LINE(sensors[5]))
            found_right = 1;
            
        // Drive straight a bit more, until we are
        // approximately in the middle of intersection.
        // This should help us better detect if we
        // have left or right segments.
        motors.setSpeeds(SPEED, SPEED);
        delay(OVERSHOOT(LINE_THICKNESS)/2);
        
        reflectanceSensors.readLine(sensors);
         
        // Check for left and right exits.
        if(ABOVE_LINE(sensors[0]))
            found_left = 1;
        if(ABOVE_LINE(sensors[5]))
            found_right = 1;
        
        // After driving a little further, we
        // should have passed the intersection
        // and can check to see if we've hit the
        // finish line or if there is a straight segment
        // ahead.   
        delay(OVERSHOOT(LINE_THICKNESS)/2);
        
        // Check for a straight exit.
        reflectanceSensors.readLine(sensors);
        
        // Check again to see if left or right segment has been found
        if(ABOVE_LINE(sensors[0]))
            found_left = 1;
        if(ABOVE_LINE(sensors[5]))
            found_right = 1;
        
        if(ABOVE_LINE(sensors[1]) || ABOVE_LINE(sensors[2]) || ABOVE_LINE(sensors[3]) || ABOVE_LINE(sensors[4]))
            found_straight = 1;
         
        // Check for the ending spot.
        // If all four middle sensors are on dark black, we have
        // solved the maze.
        if(ABOVE_LINE(sensors[1]) && ABOVE_LINE(sensors[2]) && ABOVE_LINE(sensors[3]) && ABOVE_LINE(sensors[4]))
        {
          motors.setSpeeds(0,0);
          break;
        }
         
        // Intersection identification is complete.
        unsigned char dir = selectTurn(found_left, found_straight, found_right);
        
        // Make the turn indicated by the path.
		turn(dir);
         
        // Store the intersection in the path variable.
        path[path_length] = dir;
        path_length++;
         
        // You should check to make sure that the path_length does not
        // exceed the bounds of the array.  We'll ignore that in this
        // example.
         
        // Simplify the learned path.
        simplifyPath();
         
    }
}

// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
void goToFinishLine()
{
  unsigned int sensors[6];
  int i = 0;

  // Turn around if the Zumo is facing the wrong direction.
  if(path[0] == 'B')
  {
    turn('B');
    i++;
  }
  
  for(;i<path_length;i++)
  {

    followSegment();
                  
    // Drive through the intersection. 
    motors.setSpeeds(SPEED, SPEED);
    delay(OVERSHOOT(LINE_THICKNESS));
                   
    // Make a turn according to the instruction stored in
    // path[i].
    turn(path[i]);
  }
    
  // Follow the last segment up to the finish.
  followSegment();
 
  // The finish line has been reached.
  // Return and wait for another button push to
  // restart the maze.         
  reflectanceSensors.readLine(sensors);
  motors.setSpeeds(0,0);
  
  return; 
} 


// simplifyPath analyzes the path[] array and reduces all the
// turns. For example: Right turn + Right turn = (1) Back turn.
void simplifyPath()
{
  
  // only simplify the path if the second-to-last turn was a 'B'
  if(path_length < 3 || path[path_length - 2] != 'B')
  return;
   
  int total_angle = 0;
  int i;
  
  for(i = 1; i <= 3; i++)
  {
    switch(path[path_length - i])
    {
      case 'R':
        total_angle += 90;
        break;
      case 'L':
        total_angle += 270;
        break;
      case 'B':
        total_angle += 180;
        break;
    }
  }
   
  // Get the angle as a number between 0 and 360 degrees.
  total_angle = total_angle % 360;
   
  // Replace all of those turns with a single one.
  switch(total_angle)
  {
    case 0:
      path[path_length - 3] = 'S';
      break;
    case 90:
      path[path_length - 3] = 'R';
      break;
    case 180:
      path[path_length - 3] = 'B';
      break;
    case 270:
      path[path_length - 3] = 'L';
      break;
  }
   
  // The path is now two steps shorter.
  path_length -= 2;
}

Thank You For All Your Help
Jordan V

Hello.

Another technical support representative said that you called and talked to them. He mentioned that you were going to try changing the batteries because the Zumo worked a few times and then seemed to stop sooner as time went on. Have you tried that yet?

-Brandon

Dear friends I’m having the same problem , the batteries are new and the intersection to it , how to solve , please help me .

Hello.

I suspect something in your maze might differ from what the code expects. Could you post some pictures of your maze, or a video of the Zumo trying to solve it?

-Brandon

I am using a tape to the line of 0.74 "inches , what standard should I use? If line problem , is Mr. send me a pattern saved in Word file ?.
Thank you very much in advance.

Hello.

The width of the line is defined in the Zumo maze solving example code as “LINE_THICKNESS”, and is set to 0.75 inches, so it sounds like your line thickness is the same. I would still like to see some pictures of your maze since the width of your line is not the only thing that could cause the problem.

The way the maze solving example code works is that when it comes to an intersection, it moves forward for some amount of time and checks the sensors again. If the four middle sensors are still reading a dark surface, it determines that it is at the end. The timing in the example code is based on using the 75:1 HP micro metal gearmotors, so if you are using a higher gear ratio, it might not be going far enough to pass the line, so it could be mistaking the intersection for the end of the maze. What gear ratio motors is your Zumo using? If you are not using the 75:1 HP gearmotors, you will probably need to adjust your “INCHES_TO_ZUNITS” parameter.

I am not sure what you are asking for when you mention a pattern saved in a Word file; can you try rephrasing your question?

-Brandon

I do not know which inform the motor interface, because I took with a friend , I know he is 200RPM how can I change the INCHES_TO_ZUNITS , the value it has is 17142. is attached images as requested.
Thank you very much in advance






From the RPM you mentioned it sounds like you might be using 150:1 micro metal gearmotors. If that’s the case, you might try doubling the “INCHES_TO_ZUNITS” parameter to see if that fixes the problem. If not, you can read the comments in the code to see how it is defined and do some trial and error.

-Brandon

I duplicated the value of INCHES_TO_ZUNITS and made a new track and the problem remains following file


When you say that you duplicated the value, do you mean that you doubled it? As I mentioned in my previous post, you might need to do some trial and error to find a good value. As stated in the comments of the Zumo MazeSolver example code:

// When the motor speed of the zumo is set by
// motors.setSpeeds(200,200), 200 is in ZUNITs/Second.
// A ZUNIT is a fictitious measurement of distance
// and only helps to approximate how far the Zumo has
// traveled. Experimentally it was observed that for 
// every inch, there were approximately 17142 ZUNITs.
// This value will differ depending on setup/battery
// life and may be adjusted accordingly. This value
// was found using a 75:1 HP Motors with batteries
// partially discharged.

The suggestion of doubling the INCHES_TO_ZUNITS value is also based on a guess that you are using 150:1 HP micro metal gearmotors (which is an assumption based on the RPM rating you mentioned), so you might try doing some experiments of your own as well.

Did you change the code in any other way (such as changing the speed from something other than 200)? Just to be clear, the Zumo follows the line normally, but when it gets to the intersection, it stops; is that correct? Do you hear the buzzer make a sound when it stops? If that is not the behavior you are seeing, can you post a video showing what it is doing?

-Brandon

Yes , when it for the intersection only gives a beep , following video youtu.be/y2qZPmBXI8A

Thank you for posting a video to clarify the behavior. That behavior is what I expected based on your description. Can you answer the rest of the questions from my previous post (e.g. did you double the INCHES_TO_ZUNITS value, and did you change the code in any other way)? If doubling the INCHES_TO_ZUNITS value does not work, you could try increasing it by some interval (maybe around 1000 or so) a few times to see if it makes a difference.

-Brandon