Xe tự động tránh vật cản

Cái này viết ngắn gọn.

Xe tự động tránh vật cản

Trong bài hướng dẫn này, chúng ta cùng tìm hiểu phương pháp điều khiển động cơ thông qua module điều khiển động cơ L298, cách đọc khoảng cách từ cảm biến siêu âm SRF04, cách điều khiển động cơ RC servo từ đó xây dựng mô hình xe tránh vật cản đơn giản.

Các linh kiện cần chuẩn bị

Giới thiệu module L298

  • Module điều khiển động cơ L298 là một module gồm 2 mạch cầu H tích hợp trong IC L298, nhờ đó module này có thể điều khiển được 2 động cơ riêng biệt.
  • Chân A Enable, B Enable là 2 chân điều khiển tốc độ 2 động cơ riêng biệt.
  • Input: Là 4 chân điều khiển chiều quay của 2 động cơ.
  • Bộ nguồn 12V-GND-5V: Tùy thuộc loại động cơ mà ta chọn 12V hay 5V.
  • Output A, Output B: Là 2 đầu ra kết nối với 2 động cơ.

Cảm biến siêu âm

  • Cảm biến SRF04 hoạt động dựa trên nguyên tắc phát đi một xung và tính thời gian từ lúc phát xung đến lúc nhận về được xung đó. Từ đó tính khoảng cách bằng cách với thời gian vừa đọc được nhân với vận tốc sóng siêu âm.

Động cơ RC servo

  • Động cơ RC servo là loại động cơ nhỏ, có thể khiển chính xác góc quay bằng phương pháp điều rộng xung PWM (chân có dấu ~). Tuy nhiên, nó chỉ có thể quay được góc từ 0-180 độ.

Kết nối module L298 với UnoX

  • Ở đây ta cần chú ý, module L298 có thể điều khiển được tốc độ động cơ thông qua 2 chân A Enable và B Enable. Do đó, ta cần kết nối 2 chân này với chân PWM của UnoX (chân có dấu ~)

  • Bảng kết nối (Ở đây kết nối UnoX với 2 module L298):

Module L298Board UnoX
Chân A EnableChân 6 (11)
Chân IN1Chân 8 (A0)
Chân IN2Chân 7 (13)
Chân IN3Chân 4 (12)
Chân IN4Chân 2 (9)
Chân B EnableChân 5 (10)

Kết nối UnoX với cảm biến siêu âm SRF04

  • Bảng kết nối
Module SRF04Board UnoX
Chân VCCChân 5V
Chân TrigChân A5
Chân EchoChân A4
Chân GNDChân GND

Kết nối UnoX với động cơ RC servo

  • Bảng kết nối:
RC servoBoard UnoX
Dây nâuChân GND
Dây đỏChân 5V
Dây camChân 3

Kết nối UnoX và các thiết bị

  • Từ các bảng kết nối trên chúng ta có được mô hình kết nối tổng quát như hình bên. Sau khi kết nối hoàn tất, ta đưa tất cả các module và board UnoX lên khung xe Robot.

Source code

  • Bài toán đặt ra:

    • Xe robot luôn luôn di chuyển thẳng.
    • Servo tiến hành quét các góc từ 0 - 180 độ.
    • Cảm biến siêu âm đọc tín hiệu liên tục.
    • Khi bất kỳ góc nào của servo mà cảm biến đọc giá trị nhỏ hơn 20cm thì UnoX sẽ xử lý giá trị đọc và đưa ra phương án tối ưu nhất để hạn chế việc va chạm vào vật cản trong quá trình di chuyển.
  • Sử dụng thư viện:

    • Điều khiển động cơ: điều khiển IO và PWM.
    • Đọc dữ liệu từ SRF04: điều khiển IO và đọc thời gian.
    • Điều khiển RC servo: dùng thư viện Servo.h
  //library control RC servo
  #include <Servo.h>

  //define pin of L298 left
  #define enaPinL     6
  #define in1PinL     8
  #define in2PinL     7
  #define in3PinL     4
  #define in4PinL     2
  #define enbPinL     5

  //define pin of L298 right
  #define enaPinR     11
  #define in1PinR     A0
  #define in2PinR     13
  #define in3PinR     12
  #define in4PinR     9
  #define enbPinR     10

  // define pin of SRF04
  #define trigPin     A5
  #define echoPin     A4

  //define pin of RC servo
  #define servoPin    3

  //name of RC servo
  Servo MyServo;

  void setup()
  {
    //setup pin of 2 L298 module mode output
    pinMode(enaPinL, OUTPUT);
    pinMode(in1PinL, OUTPUT);
    pinMode(in2PinL, OUTPUT);
    pinMode(in3PinL, OUTPUT);
    pinMode(in4PinL, OUTPUT);
    pinMode(enbPinL, OUTPUT);
    pinMode(enaPinR, OUTPUT);
    pinMode(in1PinR, OUTPUT);
    pinMode(in2PinR, OUTPUT);
    pinMode(in3PinR, OUTPUT);
    pinMode(in4PinR, OUTPUT);
    pinMode(enbPinR, OUTPUT);

    //setup pin of srf04
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);

    //setup pin of RC servo
    MyServo.attach(servoPin);

    //setup angle of RC servos
    MyServo.write(90);
  }

  //control single motor with speed and direction
  void controlMotor(int motor, int spd, bool dir);

  //car go straight ahead
  void Go(int spd);

  //car go back
  void Back(int spd);

  //car turn left
  void Left(int spd);

  //car turn right
  void Right(int spd);

  //car stop
  void Stop();

  //set distance from srf04
  float getDistance();

  void loop()
  {
    //scan barrier from 0 to 180 degree
    //if barrier in right -> turn left
    //if barrier in left -> turn right
    //if barrier in left, right, front -> turn back
    for (int i = 30; i <= 180; i+= 30)
    {
      MyServo.write(i);
      delay(100);
      float distance = getDistance();
      if (distance < 20.0)
      {
        float dis90;
        float dis45;
        float dis135;
        Stop();
        MyServo.write(45);
        delay(300);
        dis45 = getDistance();
        MyServo.write(90);
        delay(300);
        dis90 = getDistance();
        MyServo.write(135);
        delay(300);
        dis135 = getDistance();
        MyServo.write(90);
        if (i < 90)
        {
          if (dis45 < 20.0 && dis90 < 20.0 && dis135 < 20.0)
          {
            Right(255);
            delay(1000);
          }
          else
          {
            if (dis90 <= dis135)
            {
              Left(255);
              delay((90 - i) * 500 / 90 + 250);
            }
            else
            {
              Left(255);
              delay((90 - i) * 500 / 90);
            }
          }
        }
        else if (i = 90)
        {
          if (dis45 < 20.0 && dis90 < 20.0 && dis135 < 20.0)
          {
            Right(255);
            delay(1000);
          }
          else
          {
            if (dis45 < dis135)
            {
              Left(255);
              delay(250);
            }
            else
            {
              Right(255);
              delay(250);
            }
          }
        }
        else
        {
          if (dis45 < 20.0 && dis90 < 20.0 && dis135 < 20.0)
          {
            Right(255);
            delay(1000);
          }
          else
          {
            if (dis90 <= dis45)
            {
              Right(255);
              delay((i - 90) * 500 / 90 + 250);
            }
            else
            {
              Right(255);
              delay((i - 90) * 500 /90 + 250);
            }
          }
        }
        break;
      }
      else
        Go(255);
    }

    for (int i = 180; i >= 30; i-= 30)
    {
      MyServo.write(i);
      delay(100);
      float distance = getDistance();
      if (distance < 20.0)
      {
        float dis90;
        float dis45;
        float dis135;
        Stop();
        MyServo.write(45);
        delay(300);
        dis45 = getDistance();
        MyServo.write(90);
        delay(300);
        dis90 = getDistance();
        MyServo.write(135);
        delay(300);
        dis135 = getDistance();
        MyServo.write(90);
        if (i < 90)
        {
          if (dis45 < 20.0 && dis90 < 20.0 && dis135 < 20.0)
          {
            Right(255);
            delay(1000);
          }
          else
          {
            if (dis90 <= dis135)
            {
              Left(255);
              delay((90 - i) * 500 / 90 + 250);
            }
            else
            {
              Left(255);
              delay((90 - i) * 500 / 90);
            }
          }
        }
        else if (i = 90)
        {
          if (dis45 < 20.0 && dis90 < 20.0 && dis135 < 20.0)
          {
            Right(255);
            delay(1000);
          }
          else
          {
            if (dis45 < dis135)
            {
              Left(255);
              delay(250);
            }
            else
            {
              Right(255);
              delay(250);
            }
          }
        }
        else
        {
          if (dis45 < 20.0 && dis90 < 20.0 && dis135 < 20.0)
          {
            Right(255);
            delay(1000);
          }
          else
          {
            if (dis90 <= dis45)
            {
              Right(255);
              delay((i - 90) * 500 / 90 + 250);
            }
            else
            {
              Right(255);
              delay((i - 90) * 500 /90 + 250);
            }
          }
        }
        break;
      }
      else
        Go(255);
    }
  }

  void controlMotor(int motor, int spd, bool dir)
  {
    switch (motor)
    {
      case 1: // motor left front
        analogWrite(enaPinR, spd);
        if (dir == 0)
        {
          digitalWrite(in1PinR, HIGH);
          digitalWrite(in2PinR, LOW);
        }
        else
        {
          digitalWrite(in1PinR, LOW);
          digitalWrite(in2PinR, HIGH);
        }
        break;
      case 2: // motor left back
        analogWrite(enbPinL, spd);
        if (dir == 0)
        {
          digitalWrite(in3PinL, HIGH);
          digitalWrite(in4PinL, LOW);
        }
        else
        {
          digitalWrite(in3PinL, LOW);
          digitalWrite(in4PinL, HIGH);
        }
        break;
      case 3: // motor left front
        analogWrite(enbPinR, spd);
        if (dir == 0)
        {
          digitalWrite(in3PinR, HIGH);
          digitalWrite(in4PinR, LOW);
        }
        else
        {
          digitalWrite(in3PinR, LOW);
          digitalWrite(in4PinR, HIGH);
        }
        break;
      case 4: // motor left back
        analogWrite(enaPinL, spd);
        if (dir == 0)
        {
          digitalWrite(in1PinL, HIGH);
          digitalWrite(in2PinL, LOW);
        }
        else
        {
          digitalWrite(in1PinL, LOW);
          digitalWrite(in2PinL, HIGH);
        }
        break;
    }
  }

  void Go(int spd)
  {
    controlMotor(1, spd, 1);
    controlMotor(2, spd, 1);
    controlMotor(3, spd, 1);
    controlMotor(4, spd, 1);
  }

  void Back(int spd)
  {
    controlMotor(1, spd, 0);
    controlMotor(2, spd, 0);
    controlMotor(3, spd, 0);
    controlMotor(4, spd, 0);
  }

  void Left(int spd)
  {
    controlMotor(1, spd, 0);
    controlMotor(2, spd, 0);
    controlMotor(3, spd, 1);
    controlMotor(4, spd, 1);
  }

  void Right(int spd)
  {
    controlMotor(1, spd, 1);
    controlMotor(2, spd, 1);
    controlMotor(3, spd, 0);
    controlMotor(4, spd, 0);
  }

  void Stop()
  {
    controlMotor(1, 0, 0);
    controlMotor(2, 0, 0);
    controlMotor(3, 0, 0);
    controlMotor(4, 0, 0);
  }

  float getDistance()
  {
    float duration, distanceCm;

    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    duration = pulseIn(echoPin, HIGH);

    distanceCm = duration / 29.1 / 2;

    return distanceCm;
  }

Ứng dụng mở rộng