2

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:

The maze (.sldprt if wanted)

Pictures describing the turning problem:

robot before the corner

turning problem image 1

turning problem image 2

Images of the robot:

front of robot

right of robot

left of 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;
  }
|improve this question

migrated from stackoverflow.com Dec 7 '18 at 0:48

This question came from our site for professional and enthusiast programmers.

0

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.

|improve this answer
  • 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 – user3765883 Aug 22 at 0:50
0

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.

|improve this answer

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy