(esp32 로봇카) 장애물 회피

보통, 초음파 센서 하나를 서보 모터에 물려서 장애물 회피 기동을 하는데, 서보 모터가 전력을 많이 먹다 보니, 오동작을 하는 경우가 종종 있다. 그래서 초음파 센서 3개를 각기 좌, 정면, 우에 하나씩 달아서 장애물을 피하도록 했다. 도중에 블루투스로 제어하면 통할 때가 있고 통하지 않을 때가 있는데, 메인 loop 안에 있을 때는 블루투스 통제를 따르고, avoid_obstacle() 함수 안으로 들어가서 반복적으로 회피 기동을 하고 있다면 블루투스 명령이 먹히지 않는다. 블루투스 명령 코드가 메인 loop 안에 있기 때문이다.

 

로직은 이렇다.

전진하다가 장애물을 만날 경우(일단 40cm를 감지 거리로 설정했으나 현실에 따라 바뀔 수 있음) 40cm 이내에서 멈추게 된다. 일단 뒤로 조금 물러난다. 그리고 좌우 센서를 통해 어느 쪽이 장애물들과의 거리가 먼지 확인한 다음, 먼쪽으로 방향을 틀게 된다. 대략 1초 정도 회전을 하는데 속도 speed에 따라 회전하는 양이 달라진다. 회전 후에 다시 정면의 장애물과의 거리를 측정하고 회피 거리 이내에 있다면 다시 좌우 센서를 통해 거리를 측정해서 먼쪽으로 회전한다. 이런 식으로 반복적으로 이루어지는데, 회전한 후의 정면 장애물과의 거리가 40cm 이상일 경우, 정상 주행 모드로 돌아가서 메인 loop로 돌아간다.

 

SR-04 초음파 센서 3개를 각각, 정면, 우측, 좌측에 달아서 장애물 회피 능력을 높였다. 작은 브레드보드를 이용해 키를 올려 위치를 잡았다.

 

esp32에 확장보드를 달아서 배선을 처리하고 있다. 5V 출력이 가능하다. 왼편의 배터리는 18650 2개(7V~8.2V 출력). 이것을 L298N에 넣어주고 있다. L298N의 5V 출력을 esp32의 VIN에 넣어주고 있다.

 

 

테스트를 해보니 그럭저럭 만족스러운데 좀 복잡한 곳에서도 잘 빠져나오긴 하는데, 정면 센서 위치는 6.5cm 높이에 달려 있고, 측면 센서 둘은 바퀴 바로 위쪽, 즉 7.5cm 높이에 달려 있다. 이 높이 차이는 밑에 깔아둔 브레드 보드 한 장 차이다. 센서 위치를 조금 더 낮출 수 있다면 낮은 장애물을 피하는 데 유리할 것이다.

 

이제 주어진 각도만큼 회전하는 것과, 나침반 방향을 기준으로 동서남북 특정 방향으로 이동하는 것이 필요하다. 그리고 DC 모터 엔코더를 달아서 지정한 거리만큼 이동하는 것도 시도해볼 참이다. 라이다 센서를 달아서 자율 주행도 완성해야 하는데 slam toolbax 다루는 것은 유난히 복잡해서 하드웨어 특성을 많이 타기도 해서 걱정이다.

// for esp32 rc car

// I use 3 ultrasonic sensors (left, middle, right) instead of 1 ultrasonic sensor + servo motor.

// for timer

#include <Ticker.h>
#define CHECK_INTERVAL  (0.1)   // 10~30 frames per 1 sec
Ticker ticker;

// for the ultrasonic sensors
#define SOUND_SPEED 0.034
const int trigPin_1 = 15; 
const int echoPin_1 = 4;
const int trigPin_2 = 5; 
const int echoPin_2 = 18;
const int trigPin_3 = 16;
const int echoPin_3 = 17;

long duration_1, duration_2, duration_3;
float distance_1, distance_2, distance_3;

// for bluetooth
#include "BluetoothSerial.h"
BluetoothSerial SerialBT;
String device_name = "ESP32-4wheel";

//values for motors
// Motor A
const uint8_t ENA = 14;
const uint8_t IN1 = 27;
const uint8_t IN2 = 26;
// Motor B
const uint8_t ENB = 25;
const uint8_t IN3 = 33;
const uint8_t IN4 = 32;
int speed = 120; // default speed

uint8_t running_mode = 4; // default is stop, should be one of {0, 1, 2, 4, 5, 7, 8, 9}, 3 and 6 are speed control
String running_mode_str[10] = {"forward", "left forward", "left backward", "speed up", "stop", "stop", "speed down",
      "right backward", "right forward", "backward"};
uint8_t c = 4;

#define AVOID_DISTANCE 40  // 장애물 회피 거리 cm
#define DELAY_TIME    1500


void setup() {
  Serial.begin(115200);
  SerialBT.begin(device_name);
  Serial.printf("The device with name '%s' is started.\nNow you can pair it with Bluetooth!\n", device_name.c_str());
  
  // init ultrasonics
  pinMode(trigPin_1, OUTPUT);
  pinMode(echoPin_1, INPUT);
  pinMode(trigPin_2, OUTPUT);
  pinMode(echoPin_2, INPUT);
  pinMode(trigPin_3, OUTPUT);
  pinMode(echoPin_3, INPUT);

  // init motors
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // timer setting
  ticker.attach(CHECK_INTERVAL, check_status);
  delay(1000);
}

// bluetooth output
void check_status() {
  String status = running_mode_str[running_mode] + "   " + distance_1 + "  " + distance_2 + "  " + distance_3;
  SerialBT.println(status);
  //getDistance();
}


void forward(int speed1_, int speed2_) {
  running_mode = 0;
  analogWrite(ENA, speed1_);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENB, speed2_);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void backward(int speed1_, int speed2_) {
  running_mode = 9;
  analogWrite(ENA, speed1_);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENB, speed2_);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void left_forward(int speed_) {
  running_mode = 1;
  analogWrite(ENB, speed_); 
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void left_backward(int speed_) {
  running_mode = 2;
  analogWrite(ENA, speed_);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW); 
}

void right_forward(int speed_) {
  running_mode = 8;
  analogWrite(ENA, speed_);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW); 
}

void right_backward(int speed_) {
  running_mode = 7;
  analogWrite(ENB, speed_);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
}

void stop() {
  running_mode = 4;
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);

}

String BTreadStringUntil(char end_) {
  String result = "";
  char c_;
  while (SerialBT.available()) {
    c_ = SerialBT.read();
    if (c_ != end_) result += String(c_);
  }
  return result;
}

void getDistance(const int trigPin_, const int echoPin_, long &duration_, float &distance_) {
  digitalWrite(trigPin_, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin_, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigPin_, LOW);

  duration_ = pulseIn(echoPin_, HIGH);
  distance_ = duration_ * SOUND_SPEED / 2;

  Serial.print(distance_);
  Serial.print(" cm\t");
}

void avoid_obstacle() {
  while(1) {
    // 뒤로 조금 물러남 
    backward(speed, speed);
    delay(DELAY_TIME/2);
    stop();
    delay(DELAY_TIME/2);
    // 좌우의 장애물과의 거리를 비교해서 어디 쪽으로 돌지 결정함.
    getDistance(trigPin_1, echoPin_1, duration_1, distance_1); // 왼쪽 거리를 구함
    getDistance(trigPin_3, echoPin_3, duration_3, distance_3); // 오른쪽 거리를 구함

    if (distance_1 > AVOID_DISTANCE && distance_1 < 700 && distance_1 > distance_3) { // 왼쪽 거리가 멀다면 왼쪽으로 회전
      left_forward(speed);
      delay(DELAY_TIME);
      stop();
      delay(DELAY_TIME/2);
      getDistance(trigPin_2, echoPin_2, duration_2, distance_2); // 정면의 거리 구함
      if (distance_2 > AVOID_DISTANCE) {
        forward(speed, speed);
        return;
      } else {
        continue;
      }
    } else if (distance_3 > AVOID_DISTANCE && distance_3 < 700 && distance_3 >= distance_1) {
      right_forward(speed);
      delay(DELAY_TIME);
      stop();
      delay(DELAY_TIME/2);
      getDistance(trigPin_2, echoPin_2, duration_2, distance_2); // 정면의 거리 구함
      if (distance_2 > AVOID_DISTANCE) {
        forward(speed, speed);
        return;
      } else {
        continue;
      }
    } else {
      continue;
    }
  }
  return;
}

void loop() {
  // ultrasonic sensors - left one: 1, middle:2, right: 3
  getDistance(trigPin_2, echoPin_2, duration_2, distance_2);
  if (distance_2 > 0 && distance_2 < AVOID_DISTANCE) {
    avoid_obstacle();
  }

  if (SerialBT.available()) {
    String message = BTreadStringUntil('\n');
    //Serial.println(message);
    uint8_t c = message.toInt();
    if (c == 3) {
      SerialBT.println(running_mode_str[c]);
      speed += 10;
      if (speed > 250) speed = 250;
    } else if (c == 6) {
      SerialBT.println(running_mode_str[c]);
      speed -= 10;
      if (speed < 0) speed = 0;
    } else {
      running_mode = c;
      //Serial.print("else_running_mode: "); Serial.println(running_mode);      
    }
    //Serial.print("running_mode: "); Serial.println(running_mode);  
    switch (running_mode) {
      case 0:  // forward
        SerialBT.println(running_mode_str[running_mode]);
        forward(speed, speed);
        break;
      case 1:  // left forward
        SerialBT.println(running_mode_str[running_mode]);
        left_forward(speed);
        break;
      case 2:  // left backward
        SerialBT.println(running_mode_str[running_mode]);
        left_backward(speed);
        break;
      case 4:  // stop
      case 5:  // stop
        SerialBT.println(running_mode_str[running_mode]);
        stop();
        break;
      case 7:  // right backward
        SerialBT.println(running_mode_str[running_mode]);
        right_backward(speed);
        break;
      case 8:  // right forward
        SerialBT.println(running_mode_str[running_mode]);
        right_forward(speed);
        break;
      case 9:  //backward
        SerialBT.println(running_mode_str[running_mode]);
        backward(speed, speed);
        break;
      default:
        ;
    }
  }
}

0.1초마다 블루투스로 출력되는 동작 및 센서 정보.

이렇게 센서 3개를 사용해도 아쉬운 점이 있다. 장애물에 비스듬하게 다가갈 경우, 감지가 되지 않는다. 초음파의 입사각과 반사각이 달라서 생기는 문제 같다. 경사가 있는 구조물(높이가 높거나 낮은, 경사가 약간 있는 장애물: 현관, 담요, 신발...)을 만났을 경우, 기울기가 발생해서 바퀴가 헛돌면서 stall이 되는 문제가 있다. 충격 감지 센서를 달아서 충격이 발생한 뒤에 바퀴를 멈추고 LED를 반짝이게 하는 방법이 있겠고, 기울기가 특정 각도 이상으로 되면 경고등을 띄우기, 차의 위치가 변함이 없으면(3개 센서의 거리 정보가 오차범위 내에서 지정 시간 동안 변함이 없으면) 후진을 한다던지 하는 해결책을 고안해볼 수 있겠다. 한가지 문제점은 브레드 보드에 센서를 장착했기 때문에, 몇 번의 충격 뒤에는 접촉 불량이 발생해 센서의 거리 정보가 잘못된 경우가 종종 있다는 점이다. 이것은 PCB 기판에 납땜을 하면 해결될 문제 같은데, 장애물 회피 기동은 이 정도에서 마무리를 하는 편이 좋을 것 같다. 방위각 감지 및 이동... 등 해야 할 일이 많다.

 

https://youtube.com/shorts/NC3Qw0y0dmw?si=yIv2Vjz9ySu3-R8U

장애물을 포착하지 못한 상태에서 전진이나 후진을 계속하려고 하면 stall 상태(장애물을 밀지만 힘이 약해서 제자리 걸음 상태)에 빠지게 되는데 이를 보완한 코드는 아래와 같다.

// for esp32 rc car

// I use 3 ultrasonic sensors (left, middle, right) instead of 1 ultrasonic sensor + servo motor.

// for timer

#include <Ticker.h>
#define CHECK_INTERVAL  (0.1)   // 10~30 frames per 1 sec
Ticker ticker;

// for the ultrasonic sensors
#define SOUND_SPEED 0.034
const int trigPin_1 = 15; 
const int echoPin_1 = 4;
const int trigPin_2 = 5; 
const int echoPin_2 = 18;
const int trigPin_3 = 16;
const int echoPin_3 = 17;

long duration_1, duration_2, duration_3;
float distance_1, distance_2, distance_3;

// for bluetooth
#include "BluetoothSerial.h"
BluetoothSerial SerialBT;
String device_name = "ESP32-4wheel";

//values for motors
// Motor A
const uint8_t ENA = 14;
const uint8_t IN1 = 27;
const uint8_t IN2 = 26;
// Motor B
const uint8_t ENB = 25;
const uint8_t IN3 = 33;
const uint8_t IN4 = 32;
int speed = 150; // default speed

uint8_t running_mode = 4; // default is stop, should be one of {0, 1, 2, 4, 5, 7, 8, 9}, 3 and 6 are speed control
String running_mode_str[10] = {"forward", "left forward", "left backward", "speed up", "stop", "stop", "speed down",
      "right backward", "right forward", "backward"};
uint8_t c = 4; // incoming char from bluetooth, 4 = stop

#define AVOID_DISTANCE 40  // 장애물 회피 거리 cm
#define DELAY_TIME    1500
#define TIME_FOR_PREVENT_STALL  20000   // milli sec
unsigned long t_forward = 0;  // 전진이 몇 초나 진행되고 있는지 millis()를 담기 위한 변수
unsigned long t_backward = 0; // 후진이 몇 초나 진행되고 있는지 millis()를 담기 위한 변수

void setup() {
  Serial.begin(115200);
  SerialBT.begin(device_name);
  Serial.printf("The device with name '%s' is started.\nNow you can pair it with Bluetooth!\n", device_name.c_str());
  
  // init ultrasonics
  pinMode(trigPin_1, OUTPUT);
  pinMode(echoPin_1, INPUT);
  pinMode(trigPin_2, OUTPUT);
  pinMode(echoPin_2, INPUT);
  pinMode(trigPin_3, OUTPUT);
  pinMode(echoPin_3, INPUT);

  // init motors
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // timer setting
  ticker.attach(CHECK_INTERVAL, check_status);
  //delay(1000);
}

// bluetooth output
void check_status() {
  String status = running_mode_str[running_mode] + "   " + distance_1 + "  " + distance_2 + "  " + distance_3;
  SerialBT.println(status);
  SerialBT.flush(); // for fast output to mobile
  //getDistance();
}

void forward(int speed1_, int speed2_) {
  running_mode = 0;
  analogWrite(ENA, speed1_);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENB, speed2_);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void backward(int speed1_, int speed2_) {
  running_mode = 9;
  analogWrite(ENA, speed1_);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENB, speed2_);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void left_forward(int speed_) {
  running_mode = 1;
  analogWrite(ENB, speed_); 
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void left_backward(int speed_) {
  running_mode = 2;
  analogWrite(ENA, speed_);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW); 
}

void right_forward(int speed_) {
  running_mode = 8;
  analogWrite(ENA, speed_);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW); 
}

void right_backward(int speed_) {
  running_mode = 7;
  analogWrite(ENB, speed_);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
}

void stop() {
  running_mode = 4;
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);

}

String BTreadStringUntil(char end_) {
  String result = "";
  char c_;
  while (SerialBT.available()) {
    c_ = SerialBT.read();
    if (c_ != end_) result += String(c_);
  }
  return result;
}

void getDistance(const int trigPin_, const int echoPin_, long &duration_, float &distance_) {
  digitalWrite(trigPin_, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin_, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigPin_, LOW);

  duration_ = pulseIn(echoPin_, HIGH);
  distance_ = duration_ * SOUND_SPEED / 2;

  Serial.print(distance_);
  Serial.print(" cm\t");
}

void avoid_obstacle() {
    t_backward = millis();
  while(1) {
    // 뒤로 조금 물러남 
    backward(speed, speed);
    delay(DELAY_TIME/2);
    //stop();
    delay(DELAY_TIME/2);
    // 좌우의 장애물과의 거리를 비교해서 어디 쪽으로 돌지 결정함.
    getDistance(trigPin_1, echoPin_1, duration_1, distance_1); // 왼쪽 거리를 구함
    getDistance(trigPin_3, echoPin_3, duration_3, distance_3); // 오른쪽 거리를 구함

    if (distance_1 > AVOID_DISTANCE && distance_1 < 700 && distance_1 > distance_3) { // 왼쪽 거리가 멀다면 왼쪽으로 회전
      left_forward(speed); t_backward = millis();
      delay(DELAY_TIME);
      stop();
      delay(DELAY_TIME/2);
      getDistance(trigPin_2, echoPin_2, duration_2, distance_2); // 정면의 거리 구함
      if (distance_2 > AVOID_DISTANCE) {
        t_forward = millis();
        forward(speed, speed);
        return;
      } else {
        continue;
      }
    } else if (distance_3 > AVOID_DISTANCE && distance_3 < 700 && distance_3 >= distance_1) {
      right_forward(speed); t_backward = millis();
      delay(DELAY_TIME);
      stop();
      delay(DELAY_TIME/2);
      getDistance(trigPin_2, echoPin_2, duration_2, distance_2); // 정면의 거리 구함
      if (distance_2 > AVOID_DISTANCE) {
        t_forward = millis();
        forward(speed, speed);
        return;
      } else {
        continue;
      }
    } else {
      continue;
    }
    if (millis() - t_backward > TIME_FOR_PREVENT_STALL) {
      t_forward = millis();
      forward(speed, speed);
      return;
    }
  }
  return;
}

void loop() {
  // ultrasonic sensors - left one: 1, middle:2, right: 3
  getDistance(trigPin_2, echoPin_2, duration_2, distance_2);
  if (distance_2 > 0 && distance_2 < AVOID_DISTANCE) {
    avoid_obstacle();
  }
  // 주행 모드가 0(전진)이고, 지정 시간이 지났다면, 후진을 조금했다가 전진을 다시 함.
  if (running_mode == 0 && millis() - t_forward > TIME_FOR_PREVENT_STALL) { // 그리고 충격을 받았다면
    avoid_obstacle();
  }

  if (SerialBT.available()) {
    String message = BTreadStringUntil('\n');
    //Serial.println(message);
    uint8_t c = message.toInt();
    if (c == 3) {
      SerialBT.println(running_mode_str[c]);
      speed += 10;
      if (speed > 250) speed = 250;
    } else if (c == 6) {
      SerialBT.println(running_mode_str[c]);
      speed -= 10;
      if (speed < 0) speed = 0;
    } else {
      running_mode = c;
      //Serial.print("else_running_mode: "); Serial.println(running_mode);      
    }
    //Serial.print("running_mode: "); Serial.println(running_mode);  
    switch (running_mode) {
      case 0:  // forward
        SerialBT.println(running_mode_str[running_mode]);
        forward(speed, speed);
        break;
      case 1:  // left forward
        SerialBT.println(running_mode_str[running_mode]);
        left_forward(speed);
        break;
      case 2:  // left backward
        SerialBT.println(running_mode_str[running_mode]);
        left_backward(speed);
        break;
      case 4:  // stop
      case 5:  // stop
        SerialBT.println(running_mode_str[running_mode]);
        stop();
        break;
      case 7:  // right backward
        SerialBT.println(running_mode_str[running_mode]);
        right_backward(speed);
        break;
      case 8:  // right forward
        SerialBT.println(running_mode_str[running_mode]);
        right_forward(speed);
        break;
      case 9:  //backward
        SerialBT.println(running_mode_str[running_mode]);
        backward(speed, speed);
        break;
      default:
        ;
    }
  }
}