Obstacle avoiding robot, state machine code, help needed!

Projects Programming
1

I am attempting to set up the code for my obstacle avoiding robot, but I am running into some issues with it lagging between states. The only part of my code that I think could cause this is the sensor reading, but I could be wrong.

Robots specs:

  • Arduino UNO R3
  • HC-SR04 Ultrasonic Distance Sensor
  • L298n Motor Driver
  • 2x 6v pololu dc motors
#define LeftTread 5
#define Output1 7
#define Output2 6
#define LeftSense A5

#define RightTread 9
#define Output3 10
#define Output4 11
#define RightSense A4

#define echoPin 3
#define trigPin 4

enum STATE { 
  TRAVELING, AVOID_TURNING, AVOID_BACKING, AVOID_TRAVELING };

boolean moving = false;
boolean object = false;

// longs used for keep track of
// time based tasks
unsigned long prevObjectCheck = 0;
unsigned long startMoveBack = 0;
unsigned long startTurn = 0;
unsigned long startCreepForward = 0;

STATE currentState = TRAVELING;

void setup() {
  Serial.begin(9600);
  pinMode(LeftTread, OUTPUT);
  pinMode(Output1, OUTPUT);
  pinMode(Output2, OUTPUT);
  pinMode(LeftSense, INPUT);

  pinMode(RightTread, OUTPUT);
  pinMode(Output3, OUTPUT);
  pinMode(Output4, OUTPUT);
  pinMode(RightSense, INPUT);

  pinMode(echoPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
}

void loop() {
  // Check for an object every 500ms
  unsigned long currentMillis = millis();
  if(currentMillis - prevObjectCheck > 500) {
    prevObjectCheck = currentMillis;
    object = objectCheck();
  }

  switch (currentState) {
  case TRAVELING:
    Serial.println("TRAVELING");
    // If an object is detected, stop
    // moving, and switch states.
    if (object) {
      stop();
      currentState = AVOID_BACKING;
      break;
      // If no object, then start moving!
    }
    else if (!moving) {
      moveForward(255);
    }
    break;
  case AVOID_TURNING:
    Serial.println("AVOID_TURNING");
    if (!moving) {
      turnLeft(255);
      startTurn = millis();
      break;
    } 
    else if (currentMillis - startTurn > 2000) {
      stop();
      currentState = AVOID_TRAVELING;
    }
    break;
  case AVOID_BACKING:
    Serial.println("AVOID_BACKING");
    // Start moving backwards for 2 seconds
    if (!moving) {
      moveBackward(255);
      startMoveBack = millis();
      break;
    } 
    else if (currentMillis - startMoveBack > 2000) {
      stop();
      currentState = AVOID_TURNING;
    }
    break;
  case AVOID_TRAVELING:
    Serial.println("AVOID_TRAVELING");
    if (!moving) {
      moveForward(200);
      startCreepForward = millis();
      break;
    } 
    else if (object) {
      stop();
      currentState = AVOID_BACKING;
      break;
    } 
    else if (currentMillis - startCreepForward > 1000) {
      currentState = TRAVELING;
    }
    break;
  default: 
    break;
  }
}

// Check for an object by getting 5 valid
// sensor readings, and averaging distance.
boolean objectCheck() {
  Serial.println("Checking for object! Please wait!");
  int x = 0;
  int y = 0;
  int total = 0;
  do {
    y++;
    int distance = getDistance();
    if (distance < 200) {
      x++;
      total = total + distance;
    }
  } 
  while (x < 3 || y < 6);

  if ((total/x) < 25) return true;
  else return false;
}

int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  int duration = pulseIn(echoPin, HIGH);
  return duration/58.2;
}

void moveForward(int rate) {
  moving = true;
  digitalWrite(Output1, LOW);
  digitalWrite(Output2, HIGH);
  digitalWrite(Output3, LOW);
  digitalWrite(Output4, HIGH);
  analogWrite(LeftTread, rate);
  analogWrite(RightTread, rate);
}

void moveBackward(int rate) {
  moving = true;
  digitalWrite(Output1, HIGH);
  digitalWrite(Output2, LOW);
  digitalWrite(Output3, HIGH);
  digitalWrite(Output4, LOW);
  analogWrite(LeftTread, rate);
  analogWrite(RightTread, rate);
}

void turnLeft(int rate) {
  moving = true;
  digitalWrite(Output1, LOW);
  digitalWrite(Output2, HIGH);
  digitalWrite(Output3, HIGH);
  digitalWrite(Output4, LOW);
  analogWrite(LeftTread, rate);
  analogWrite(RightTread, rate);
}

void turnRight(int rate) {
  moving = true;
  digitalWrite(Output1, HIGH);
  digitalWrite(Output2, LOW);
  digitalWrite(Output3, LOW);
  digitalWrite(Output4, HIGH);
  analogWrite(LeftTread, rate);
  analogWrite(RightTread, rate);
}

void stop() {
  moving = false;
  digitalWrite(Output1, LOW);
  digitalWrite(Output2, LOW);
  digitalWrite(Output3, LOW);
  digitalWrite(Output4, LOW);
  analogWrite(LeftTread, 0);
  analogWrite(RightTread, 0);
}
2

Your echo check doesn't have a timeout. I think the default timeout is 1 second.

3

You might also get a better idea of what is going on by time stamping your debug prints.

4

johnwasser:
Your echo check doesn't have a timeout. I think the default timeout is 1 second.

You were correct, the timeout was causing my lag. When I switched it to a ~0.5ms timeout, I was able to debug the rest of my issues! Thanks for the help.

5

Jeebiss:
I am attempting to set up the code for my obstacle avoiding robot, but I am running into some issues with it lagging between states. The only part of my code that I think could cause this is the sensor reading, but I could be wrong.

case AVOID_TURNING:

Serial.println("AVOID_TURNING");
    if (!moving) {
      turnLeft(255);
      startTurn = millis();
      break;
    }
    else if (currentMillis - startTurn > 2000) {
      stop();
      currentState = AVOID_TRAVELING;
    }
    break;

If moving (the else part of the statement), where do you reset startTurn = millis()?
Same in AVOID_TRAVELING.