Maze solving robot turning failure due to sensor distance to the corner increasing during the turn

Question

At my school we have a project where we have to make a robot that can solve a 3D-maze using an arduino controlled robot that uses ultrasonic sensors to determine how to turn. The robot may not be pre-programmed and must work on all simple mazes with 90d turns.

I have figured out most of the code I think, however my problem is that when the robot needs to turn. Since how much and which direction the robot should turn is determined by distance sensors, the distance to the corners increase, making it longer than what is required to turn. This makes the robot want to turn back into the corner and then it gets stuck in a loop of turning in and out of the corner (see image attached).

Note: we don't want to program a 90d turn as it could mess up later and we believe it better to try make how long the robot should turn be determined by the value of the different sensors.

The robot is made up of three ultrasonic sensors, front, right, and left. The robot has tracks which lets it turn on the spot. The arduino is an arduino mega, so it is fast at updating and measuring values (unless the time needed for measuring the distance is long).

Files:

Pictures describing the turning problem:

Images of the robot:

If any other information is needed that is left out, I will try to obtain it (such as images, dimensions, exact component names, etc.)

The entire code:

enum robState {
  robInit,
  robInitForward,
  robInitRotL,
  robInitRotR,
  robStop,
  //robWaitCrystals,
  robInitBack,
  robReset,
  robFindWay
};

struct LFR {
  int L;
  int F;
  int R;
};

struct robMotor {
  byte pwm;
  byte dir_1;
  byte dir_2;
};

robState robStatus;
//const int robTurnSpeed = 255;
robMotor motorL = {2, 4, 3};
robMotor motorR = {5, 6, 7};

int dirInt = 1;
LFR echoPin = {12, 11, 13};
LFR trigPin = {24, 26, 22};
LFR distance;
const int robStartStopPin;

void setup() {
  // put your setup code here, to run once:

  //init pinModes
  initPins();

  Serial.begin(9600);
  robStatus = robInit;
}

void loop() {
  int disWall = 12;
  // put your main code here, to run repeatedly:
  distance.L = distanceUpdater(trigPin.L, echoPin.L);
  distance.F = distanceUpdater(trigPin.F, echoPin.F);
  distance.R = distanceUpdater(trigPin.R, echoPin.R);
  Serial.print("/n distance.L= ");
  Serial.println(distance.L);
  Serial.print("/n distance.F= ");
  Serial.println(distance.F);
  Serial.print("/n distance.R= ");
  Serial.println(distance.R);                                                                                                                                                                     
  //delayMicroseconds(50);

  switch (robStatus) {

    case robInit:
      //init motors and shit
      motor(0, 0);
      motor(1, 0);
      //wait for start/stop
      //while (digitalRead(robStartStopPin) == LOW);
      Serial.print("/n robInit");
      robStatus = robFindWay;
      break;

    case robInitForward:
      motor(0, 1);
      motor(1, 1);
      Serial.print("/n robInitForward");
      robStatus = robFindWay;
      break;

    case robFindWay:

      Serial.print("/n robFindWay");
      if (distance.R > disWall) {
        //if open to right, turn right.
        robStatus = robInitRotR;
      }
      else if ((distance.R < disWall) && (distance.F > disWall)) {
        //go forward if you can't go right
        robStatus = robInitForward;
      }

      else if ((distance.R < disWall) && (distance.F < disWall) && (distance.L > disWall)) {
        //if nothing else works, turn left.
        robStatus = robInitRotL;
      }
      else if ((distance.R < disWall) && (distance.F < disWall) && (distance.L < disWall)) {
        //found the end, must turn. turns left.
        robStatus = robInitRotL;
      }
      else {
        Serial.print("shit somethings wrong with path finding");
      }
      break;

    case robInitRotL:
      Serial.print("robInitRotL");
      //forward direction of motorR
      motor(1, 1);
      //reverse direction of motorL
      motor(0, -1);
      //send the correct motor speed*/
      robStatus = robFindWay;
      /*analogWrite(motorR.pwm, robTurnSpeed);
        analogWrite(motorL.pwm, robTurnSpeed);*/
      break;

    case robInitRotR:
      Serial.print("robInitRotR");
      //reverse direction of motorR
      motor(1, -1);
      //forward direction of motorL
      motor(0, 1);
      //send right motor speed
      robStatus = robFindWay;
      /*analogWrite(motorR.pwm, robTurnSpeed);
        analogWrite(motorL.pwm, robTurnSpeed);*/
      break;

    case robStop:
      motor(0, 0);
      motor(1, 0);
      //while (digitalRead(robStartStopPin) == LOW);
      robStatus = robInit;
      break;

    case robInitBack:
      //reverse both motors
      motor(0, -1);
      motor(1, -1);
      break;
  }


}

void initPins() {
  //init motors
  pinMode(motorL.pwm, OUTPUT);
  pinMode(motorL.dir_1, OUTPUT);
  pinMode(motorL.dir_2, OUTPUT);

  pinMode(motorR.pwm, OUTPUT);
  pinMode(motorR.dir_1, OUTPUT);
  pinMode(motorR.dir_2, OUTPUT);
  //init sensors
  pinMode(trigPin.L, OUTPUT);
  pinMode(trigPin.F, OUTPUT);
  pinMode(trigPin.R, OUTPUT);

  pinMode(echoPin.L, INPUT);
  pinMode(echoPin.F, INPUT);
  pinMode(echoPin.R, INPUT);
  //init button
  pinMode(robStartStopPin, INPUT);
  //init power sources
  /*pinMode(VCCPin1, OUTPUT);
  digitalWrite(VCCPin1, HIGH);
  pinMode(GNDPin1, OUTPUT);
  digitalWrite(GNDPin1, LOW);*/
}

void motor(int side, int action) {
  //side 0=L 1=R
  //action -1=reverse 0=still 1=forward
  int normalSpeed = 255;
  if ((side == 0) && (action == -1)) {
    //reverse left
    digitalWrite(motorL.dir_1, LOW);
    digitalWrite(motorL.dir_2, HIGH);
    analogWrite(motorL.pwm, normalSpeed);
  }
  else if ((side == 0) && (action == 0)) {
    //still left
    digitalWrite(motorL.dir_1, HIGH);
    digitalWrite(motorL.dir_2, LOW);
    analogWrite(motorL.pwm, 0);
  }
  else if ((side == 0) && (action == 1)) {
    //forward left
    digitalWrite(motorL.dir_1, HIGH);
    digitalWrite(motorL.dir_2, LOW);
    analogWrite(motorL.pwm, normalSpeed);
  }
  else if ((side == 1) && (action == -1)) {
    //reverse right
    digitalWrite(motorR.dir_1, LOW);
    digitalWrite(motorR.dir_2, HIGH);
    analogWrite(motorR.pwm, normalSpeed);
  }
  else if ((side == 1) && (action == 0)) {
    //still right
    digitalWrite(motorR.dir_1, HIGH);
    digitalWrite(motorR.dir_2, LOW);
    analogWrite(motorR.pwm, 0);
  }
  else if ((side == 1) && (action == 1)) {
    //forward right
    digitalWrite(motorR.dir_1, HIGH);
    digitalWrite(motorR.dir_2, LOW);
    analogWrite(motorR.pwm, normalSpeed);
  }
  else {
    Serial.print("monkaS wrong motor inputs");
  }
}


int distanceUpdater(int trigPin, int echoPin) {
  long duration;
  int distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration * 0.034 / 2);
  return distance;
}

The pathfinding and turning:

switch (robStatus) {

    case robInit:
      //init motors and shit
      motor(0, 0);
      motor(1, 0);
      //wait for start/stop
      //while (digitalRead(robStartStopPin) == LOW);
      Serial.print("/n robInit");
      robStatus = robFindWay;
      break;

    case robInitForward:
      motor(0, 1);
      motor(1, 1);
      Serial.print("/n robInitForward");
      robStatus = robFindWay;
      break;

    case robFindWay:

      Serial.print("/n robFindWay");
      if (distance.R > disWall) {
        //if open to right, turn right.
        robStatus = robInitRotR;
      }
      else if ((distance.R < disWall) && (distance.F > disWall)) {
        //go forward if you can't go right
        robStatus = robInitForward;
      }

      else if ((distance.R < disWall) && (distance.F < disWall) && (distance.L > disWall)) {
        //if nothing else works, turn left.
        robStatus = robInitRotL;
      }
      else if ((distance.R < disWall) && (distance.F < disWall) && (distance.L < disWall)) {
        //found the end, must turn. turns left.
        robStatus = robInitRotL;
      }
      else {
        Serial.print("shit somethings wrong with path finding");
      }
      break;

    case robInitRotL:
      Serial.print("robInitRotL");
      //forward direction of motorR
      motor(1, 1);
      //reverse direction of motorL
      motor(0, -1);
      //send the correct motor speed*/
      robStatus = robFindWay;
      /*analogWrite(motorR.pwm, robTurnSpeed);
        analogWrite(motorL.pwm, robTurnSpeed);*/
      break;

    case robInitRotR:
      Serial.print("robInitRotR");
      //reverse direction of motorR
      motor(1, -1);
      //forward direction of motorL
      motor(0, 1);
      //send right motor speed
      robStatus = robFindWay;
      /*analogWrite(motorR.pwm, robTurnSpeed);
        analogWrite(motorL.pwm, robTurnSpeed);*/
      break;

    case robStop:
      motor(0, 0);
      motor(1, 0);
      //while (digitalRead(robStartStopPin) == LOW);
      robStatus = robInit;
      break;

    case robInitBack:
      //reverse both motors
      motor(0, -1);
      motor(1, -1);
      break;
  }

Answer 1

Your main loop (loop()) is powering through the whole code continuously. It wouldn't be a bad idea to put some "breaks" between each sensor scan cycle. For example:

long timerCounter = 0;

void setup()
{
    // Do your setup here

    timerCounter = millis();
}

void loop()
{
    // Only do some code every 1 second (1000ms)
    if (millis() - timerCounter >= 1000)
    {
        ScanSensor1();
        ScanSensor2();
        ScanSensor3();

        robState state = DetermineStateFromSensorReadings();

        DoActionForState(state);
    }
}

What this does is break the cycle down to 1Hz (change as you wish). When you are using ultrasonics (as per your images), it's always a good idea to reduce the intervals between each reading, as it allows residual noise to quieten down. Imagine the beeping sound from the sonar on a submarine.

You've got the right idea by using a state machine in your code. Each cycle can determine which state it should be in after reading the sensor data. Good job on that!

Just some additional information about ultrasonics regarding your project:

• Sonar has an emission angle of 30 degrees (not a straight line!). Imagine a code pointing out with an angle of 30 degrees. This means anything inside this range could be picked up by the sensor. Looking at your turning problem image 2, I'd expect a lot of erroneous data from that sensor pointing towards the corner.
• Multiple sensors working at the same frequency (40kHz?) may interfere with each other.

The problem you are seeing is mainly caused by the fact that you are checking the walls around you as you are turning. I would suggest you use either the forward facing sensor or the opposite sensor to that in turning problem image 2 to significantly reduce erroneous data.

If your budget (and time) allows, you may benefit from using a cheap Lidar (like this one for example) to more accurately determine your surrounding. Lidar doesn't have an emission angle, which means that it's measurement point is in a straight line from the source.