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

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;
}
``````

## 2 Answers

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.

• Autonomous robots are a lot of fun (and hard!) Right now your robot is doing exactly what you told it to do - follow the wall. One way or another you are going to have to 'program in a 90-deg turn'. The hard way to do this is to model the corner, in terms of the expected distance profile vs time, and constantly adjust the robot's offset target to smoothly navigate the turn. The easy way would be to let the robot recognize that a turn is required, execute the turn, and then reassess the situation. FMI, see the 'robots' category at fpaynter.com Aug 22, 2019 at 0:50

You need two states. Making a turn and not making a turn.

Once you start making a turn, ignore the side sensors, and continue turning until the front sensor distance has reached its peak.