# 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
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);
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
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);
robStatus = robInit;
break;

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

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()
{

timerCounter = millis();
}

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

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!