Help With a Student Project

[Chimdia]

Hi All, I recently bought the Zumo 32U4 to implement a student project that involves moving and traversing a particular path. This path is a particular black line on a white background .
The way I programmed it is to use the encoder to deduce distance covered and then use the qtr sensor to follow the black line. I then developed cases representing decisions it should make at different points/junctions. Here is the map we are asked to follow, and also, here is my code.

The question is, what is the best way to achieve this robot behavior and have it follow a predefined path with precision and accuracy?

#include <Wire.h>
#include <Zumo32U4.h>

// Zumo components
Zumo32U4Buzzer buzzer;
Zumo32U4LineSensors lineSensors;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;
Zumo32U4Encoders encoders;
Zumo32U4OLED display;

// Constants
const uint16_t maxSpeed = 200;
const float wheelDiameterCM = 3.5;
const float wheelCircumferenceCM = PI * wheelDiameterCM;
const int encoderCPR = 12;
const int gearRatio = 75;
const int countsPerRev = encoderCPR * gearRatio;
const float distancePerCountCM = wheelCircumferenceCM / countsPerRev;

#define NUM_SENSORS 5
unsigned int lineSensorValues[NUM_SENSORS];
int16_t lastError = 0;

// Case 1 logic
const float expectedDistanceCM_case1 = 6.8;
const float tolerance = 0.20;
const float lowerBound_case1 = expectedDistanceCM_case1 * (1.0 - tolerance); // ~5.44
const float upperBound_case1 = expectedDistanceCM_case1 * (1.0 + tolerance); // ~8.16

// Case 2 logic
const float expectedDistanceCM_case2 = 43.2;
const float lowerBound_case2 = expectedDistanceCM_case2 * (1.0 - tolerance); // ~34.56
const float upperBound_case2 = expectedDistanceCM_case2 * (1.0 + tolerance); // ~51.84

// Case 3 logic
const float expectedDistanceCM_case3 = 80.0;
const float lowerBound_case3 = expectedDistanceCM_case3 * (1.0 - tolerance); // ~64.0
const float upperBound_case3 = expectedDistanceCM_case3 * (1.0 + tolerance); // ~96.0

bool case1Done = false;
bool case2Done = false;
bool case3Done = false;
int caseCounter = 0;

// Custom character loading (optional)
void loadCustomCharacters()
{
  static const char levels[] PROGMEM = {
    0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63
  };
  display.loadCustomCharacter(levels + 0, 0);
  display.loadCustomCharacter(levels + 1, 1);
  display.loadCustomCharacter(levels + 2, 2);
  display.loadCustomCharacter(levels + 3, 3);
  display.loadCustomCharacter(levels + 4, 4);
  display.loadCustomCharacter(levels + 5, 5);
  display.loadCustomCharacter(levels + 6, 6);
}

void calibrateSensors()
{
  delay(1000);
  for (uint16_t i = 0; i < 120; i++)
  {
    if (i > 30 && i <= 90)
      motors.setSpeeds(-200, 200);
    else
      motors.setSpeeds(200, -200);

    lineSensors.calibrate();
  }
  motors.setSpeeds(0, 0);
}

void setup()
{
  Serial.begin(9600);
  lineSensors.initFiveSensors();
  loadCustomCharacters();
  display.clear();
  display.print(F("Press A"));
  display.gotoXY(0, 1);
  display.print(F("to calib"));
  buttonA.waitForButton();

  calibrateSensors();

  // Wait for start signal
  display.clear();
  display.print(F("Ready..."));
  display.gotoXY(0, 1);
  display.print(F("Press A to go"));
  while (!buttonA.getSingleDebouncedPress());

  display.clear();
  display.print(F("Go!"));
  buzzer.play("L16 cdegreg4");
  while (buzzer.isPlaying());

  encoders.getCountsAndResetLeft();
  encoders.getCountsAndResetRight();
}

void loop()
{
  // PID Line Following
  int16_t position = lineSensors.readLine(lineSensorValues);
  int16_t error = position - 2000;
  int16_t speedDiff = error / 4 + 6 * (error - lastError);
  lastError = error;

  int16_t leftSpeed = maxSpeed + speedDiff;
  int16_t rightSpeed = maxSpeed - speedDiff;

  leftSpeed = constrain(leftSpeed, 0, (int16_t)maxSpeed);
  rightSpeed = constrain(rightSpeed, 0, (int16_t)maxSpeed);

  motors.setSpeeds(leftSpeed, rightSpeed);

  // Distance Tracking
  static uint16_t lastDisplayTime = 0;
  if ((uint16_t)(millis() - lastDisplayTime) >= 100)
  {
    lastDisplayTime = millis();

    int16_t countsLeft = encoders.getCountsLeft();
    int16_t countsRight = encoders.getCountsRight();
    float distLeft = countsLeft * distancePerCountCM;
    float distRight = countsRight * distancePerCountCM;
    float avgDistance = (distLeft + distRight) / 2.0;

    // Display distance
    display.clear();
    display.print(F("Dist:"));
    display.gotoXY(0, 1);
    display.print(avgDistance, 1);
    display.print(F(" cm"));

    // Serial debug
    Serial.print("L: ");
    Serial.print(distLeft, 2);
    Serial.print(" cm, R: ");
    Serial.print(distRight, 2);
    Serial.print(" cm, Avg: ");
    Serial.print(avgDistance, 2);
    Serial.println(" cm");

    // === CASE 1 Detection ===
    if (!case1Done)
    {
      bool allSensorsBlack = true;
      for (int i = 0; i < NUM_SENSORS; i++) {
        if (lineSensorValues[i] < 900) {
          allSensorsBlack = false;
          break;
        }
      }

      if (allSensorsBlack && avgDistance >= lowerBound_case1 && avgDistance <= upperBound_case1)
      {
        // CASE 1 Triggered: Turn right
        motors.setSpeeds(100, -100);
        delay(400);
        motors.setSpeeds(0, 0);

        caseCounter++;
        case1Done = true;

        display.clear();
        display.print(F("Case 1: OK"));
        display.gotoXY(0, 1);
        display.print(F("Turn Right"));
        Serial.println("Case 1: True. Turned Right.");
      }
    }

    // === CASE 2 Detection ===
    else if (!case2Done)
    {
      bool rightHeavy = lineSensorValues[3] > 900 && lineSensorValues[4] > 900 &&
                        lineSensorValues[0] < 500 && lineSensorValues[1] < 500;

      if (rightHeavy && avgDistance >= lowerBound_case2 && avgDistance <= upperBound_case2)
      {
        // CASE 2 Triggered: Turn right
        motors.setSpeeds(100, -100);
        delay(400);
        motors.setSpeeds(0, 0);

        caseCounter++;
        case2Done = true;

        display.clear();
        display.print(F("Case 2: OK"));
        display.gotoXY(0, 1);
        display.print(F("Turn Right"));
        Serial.println("Case 2: True. Turned Right.");
        // Removed while(1); so Case 3 can be reached
      }
    }

    // === CASE 3 Detection ===
    else if (!case3Done && case1Done && case2Done)
    {
      bool rightHeavy = lineSensorValues[3] > 900 && lineSensorValues[4] > 900 &&
                        lineSensorValues[0] < 500 && lineSensorValues[1] < 500;

      if (rightHeavy && avgDistance >= lowerBound_case3 && avgDistance <= upperBound_case3)
      {
        // CASE 3 Triggered: Turn right
        motors.setSpeeds(100, -100);
        delay(400);
        motors.setSpeeds(0, 0);

        caseCounter++;
        case3Done = true;

        display.clear();
        display.print(F("Case 3: OK"));
        display.gotoXY(0, 1);
        display.print(F("Turn Right"));
        Serial.println("Case 3: True. Turned Right.");
        // You can add more actions here if needed
      }
    }
  }
}

[BrandonM]

Hello.

I moved your post to a more appropriate section of the forum.

You might consider doing something similar to our MazeSolver example program. That example is intended for simple non-looping mazes, but the method for traversing a set path after the maze is mapped is similar. The first time through the maze, the example collects data about the turns it makes at each intersection and stores them in an array. Then (after simplifying the array to get rid of any dead-ends), when it goes through again, it detects each time it reaches an intersection and follows the path indicated by the array. Since you already know the maze layout, you could have some pre-filled arrays storing paths (skipping the first part of the MazeSolver example), and just select the correct path for each situation.

For example, to get from “D” to “END 1” your array might look something like [R, S, L, R] (i.e. right, straight, left, right).

Brandon

[Chimdia]

Wow… Thank you for shedding so much light. I will look into this method today and see the progress I can make.

[Chimdia]

Thank you very much for the kind and very prompt response. This behavioral solution works; however, I would also like to know if there is a solution I can use to implement localization and mapping. This simply means programming the robot to know its location on the map and make its turn decisions based on this knowledge.

[BrandonM]

I recommend looking into different pathfinding algorithms, such as the A star algorithm which is commonly used for looped maze solving. You might also find my post here helpful as well; it has details of the methods I used in a 2014 looped maze solving competition we did with our local robotics club.

Brandon