Robot tránh vật cản

Có thể bạn quan tâm

Xin chào các bạn yêu thích lập trình và robot! Hôm nay chúng ta sẽ cùng nhau tạo ra một robot tránh vật cản đơn giản nhưng vô cùng thú vị. Đây là một dự án rất phổ biến trong lĩnh vực robot và có thể được ứng dụng trong nhiều lĩnh vực khác nhau.

Bạn đang xem: Robot tránh vật cản

#include 

#define trig 3 //chân trig của HC-SR04
#define echo 4 //chân echo của HC-SR04

Servo srf05; // create servo object to control a servo

#define inA1 6 //Định nghĩa chân in1 của động cơ A
#define inA2 7 //Định nghĩa chân in2 của động cơ A
#define inB1 8 //Định nghĩa chân in1 của động cơ B
#define inB2 9 //Định nghĩa chân in2 của động cơ B

void setup() {
  pinMode(inA1, OUTPUT); //Set chân in1 của dc A là output
  pinMode(inA2, OUTPUT); //Set chân in2 của dc A là output
  pinMode(inB1, OUTPUT); //Set chân in1 của dc B là output
  pinMode(inB2, OUTPUT); //Set chân in2 của dc B là output
  pinMode(trig, OUTPUT); //chân trig sẽ phát tín hiệu
  pinMode(echo, INPUT); //chân echo sẽ nhận tín hiệu

  Serial.begin(9600);
  srf05.attach(5); // attaches the servo on pin 9 to the servo object
}

void loop() {
  objectAvoider(inA1, inA2, inB1, inB2, 50, 1000);
}

int objectDistance_cm(byte angle) {
  srf05.write(angle);
  delay(500);
  unsigned long duration; //biến đo thời gian
  int distance; //biến lưu khoảng cách

  digitalWrite(trig, 0); //tắt chân trig
  delayMicroseconds(2);
  digitalWrite(trig, 1); // phát xung từ chân trig
  delayMicroseconds(5); // xung có độ dài 5 microSeconds
  digitalWrite(trig, 0); //tắt chân trig

  duration = pulseIn(echo, HIGH); //đo độ rộng xung HIGH ở chân echo. (http://arduino.vn/reference/pulsein)
  distance = int(duration / 2 / 29.412); //tính khoảng cách đến vật.

  return distance;
}

void robotMover(byte inR1, byte inR2, byte inL1, byte inL2, byte action) {
  switch (action) {
    case 0: // không di chuyển
      motorControlNoSpeed(inR1, inR2, 0);
      motorControlNoSpeed(inL1, inL2, 0);
      break;
    case 1: // đi thẳng
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 1);
      break;
    case 2: // lùi lại
      motorControlNoSpeed(inR1, inR2, 2);
      motorControlNoSpeed(inL1, inL2, 2);
      break;
    case 3: // quay trái
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 2);
      break;
    case 4: // quay phải
      motorControlNoSpeed(inR1, inR2, 2);
      motorControlNoSpeed(inL1, inL2, 1);
      break;
    case 5: // rẽ trái
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 0);
      break;
    case 6: // rẽ phải
      motorControlNoSpeed(inR1, inR2, 0);
      motorControlNoSpeed(inL1, inL2, 1);
      break;
    case 7: // rẽ lùi trái
      motorControlNoSpeed(inR1, inR2, 2);
      motorControlNoSpeed(inL1, inL2, 0);
      break;
    case 8: // rẽ lùi phải
      motorControlNoSpeed(inR1, inR2, 0);
      motorControlNoSpeed(inL1, inL2, 2);
      break;
    default:
      action = 0;
  }
}

void motorControlNoSpeed(byte in1, byte in2, byte direct) {
  switch (direct) {
    case 0: // Dừng không quay
      digitalWrite(in1, LOW);
      digitalWrite(in2, LOW);
      break;
    case 1: // Quay chiều thứ 1
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      break;
    case 2: // Quay chiều thứ 2
      digitalWrite(in1, LOW);
      digitalWrite(in2, HIGH);
      break;
  }
}

void objectAvoider(byte inR1, byte inR2, byte inL1, byte inL2, byte allow_distance, int turn_back_time) {
  robotMover(inR1, inR2, inL1, inL2, 1);
  Serial.println("Tiến");

  int front_distance = objectDistance_cm(90);
  int left_distance;
  int right_distance;
  int max_distance;

  if (front_distance > allow_distance) {
    robotMover(inR1, inR2, inL1, inL2, 1);
    Serial.println("Tiến");
    delay(10);
  }

  if (front_distance <= allow_distance) {
    robotMover(inR1, inR2, inL1, inL2, 2);
    Serial.println("Lùi");
    delay(300);
    robotMover(inR1, inR2, inL1, inL2, 0);
    left_distance = objectDistance_cm(180); //đo khoảng cách bên trái
    Serial.println("left: ");
    Serial.println(left_distance);
    right_distance = objectDistance_cm(0); //đo khoảng cách bên phải
    Serial.println("right: ");
    Serial.println(right_distance);
    Serial.println("front: ");
    Serial.println(front_distance);
    max_distance = max(left_distance, right_distance);

    if (max_distance == left_distance) {
      robotMover(inR1, inR2, inL1, inL2, 3);
      Serial.println("Rẽ trái");
      delay(turn_back_time / 2);
    } else {
      if (max_distance == right_distance) {
        robotMover(inR1, inR2, inL1, inL2, 4);
        Serial.println("Rẽ phải");
        delay(turn_back_time / 2);
      }
    }
  }
}

Xem thêm : Sơ đồ chân ESP8266 – Hướng dẫn sử dụng chân đúng cách

Mình viết bằng codebender cho các bạn dễ biên dịch nha!

Cảm ơn các bạn đã theo dõi bài viết này. Hy vọng rằng bạn đã tìm thấy nó thú vị và có thể áp dụng trong các dự án của mình. Hãy luôn khám phá và học hỏi thêm nhiều điều mới mẻ về lĩnh vực robot và lập trình. Chúc các bạn thành công!

Nguồn: https://cite.edu.vn
Danh mục: Học tập

Lập trình

Related articles