Bài 7 - Làm xe dò line cơ bản.
Jump to navigation
Jump to search
Chuẩn bị.
Thư viện:
Chương trình sử dụng thư viện "SimpleMotor.h".
Các bạn cần đảm bảo đã cài đặt thêm thư viện này vào IDE Arduino.
Hướng dẫn chi tiết có trong phần: Hướng dẫn sử dụng phần mềm Arduino.
Kiểm tra căn chỉnh đầu dò của xe:
Độ chính xác hoạt động của xe dò line dựa trên các cảm biến dò line.
Vì vậy để xe hoạt động đúng, bạn cần đảm bảo lắp đặt phần đầu khung xe này chính xác.
Hướng dẫn chi tiết có trong phần: Các bài thực hành → Phần cứng.
Nạp chương trình.
Các bạn mở IDE Arduino lên, tạo Sketch mới (Ctrl+N) và copy đoạn code sau vào.
/* ------------------------------------------------------------------------- */
/* LIBRARY */
/* ------------------------------------------------------------------------- */
#include "SimpleMotor.h"
/* ------------------------------------------------------------------------- */
/* DEFINE (DRIVER) */
/* ------------------------------------------------------------------------- */
/**
* PIN_IN2 kết nối Driver L298
*
* Phần cấp nguồn cho Driver
* +12V - ... có thể cấp nguồn trong khoảng 9V~6V
* GND - GND
*
* Dùng Jumper kết nối (mặc định)
* ENA - 5V
* ENB - 5V
*
* L298 : Arduino
* IN1 : D4
* IN2 : D5 (~)
* IN3 : D6 (~)
* IN4 : D7
*/
#define PIN_IN1 4 //! D4
#define PIN_IN2 5 //! D5 (~)
#define PIN_IN3 6 //! D6 (~)
#define PIN_IN4 7 //! D7
/* ------------------------------------------------------------------------- */
/* DEFINE (SENSOR) */
/* ------------------------------------------------------------------------- */
/**
* Pin kết nối Thanh dò line TCRT5000
*
* TCRT5000 : Arduino
* GND - GND
* 5V - 5V
* OUT1 - A1 (Digital)
* OUT2 - A2 (Digital)
*/
#define PIN_OUT1 A1 //! A1
#define PIN_OUT2 A2 //! A2
#define PIN_OUT3 A3 //! A3
/* ------------------------------------------------------------------------- */
/* DEFINE (CAR) */
/* ------------------------------------------------------------------------- */
// Tốc độ motor, đơn vị PWM (0-255)
#define PER_100 255
#define PER_90 230
#define PER_80 205
#define PER_70 179
#define PER_60 154
#define PER_50 128
#define PER_40 102
#define PER_30 77
#define PER_20 51
#define PER_10 26
#define PER_0 0
// Các mức tốc độ xe sẽ sử dụng
#define FAST PER_60 //!
#define NORMAL PER_50 //!
#define SLOW PER_40 //!
/**
* Khoảng thời gian "timeout"
*
* Là khoảng thời gian cho phép xe vẫn chạy tiếp dù đã ra ngoài line
* Đây là khoảng thời gian để xe tìm lại đường line
*
* Hết thời gian mà xe vẫn ko thấy line, sẽ cho xe dừng hẳn
*/
#define TIME_OUT 750 //! Đơn vị (ms)
/* ------------------------------------------------------------------------- */
/* VARIABLE */
/* ------------------------------------------------------------------------- */
// Khởi tạo kết nối các chân với Driver
SimpleMotor car(PIN_IN1, PIN_IN2, PIN_IN3, PIN_IN4);
// Lưu giá trị đọc từ cảm biến dò line
bool lineRight; // Cho OUT1
bool lineCenter; // Cho OUT2
bool lineLeft; // Cho OUT3
/**
* Cho biết hướng lệch hiện tại của xe
* Lệch Phải mức 2 : +2
* Lệch Phải mức 1 : +1
* Ngay giữa line : 0
* Lệch Trái mức 1 : -1
* Lệch Trái mức 2 : -2
*/
int direction;
/**
* Cho biết xe đang trong line hay ngoài line
* Trong line : TRUE
* Ngoài line : FALSE
*/
bool inLine;
// Lưu thời điểm phát hiện mỗi khi xe vừa ra ngoài line
unsigned long capturePoint;
/* ------------------------------------------------------------------------- */
/* FUNCTION */
/* ------------------------------------------------------------------------- */
void read_sensor()
{
lineRight = digitalRead(PIN_OUT1);
lineCenter = digitalRead(PIN_OUT2);
lineLeft = digitalRead(PIN_OUT3);
}
void level_line()
{
if (!lineLeft && !lineCenter && lineRight) // 001
{
/**
* Lệch trái nhiều ... rẽ phải ... bánh trái quay nhanh hơn nữa
*/
direction = -2;
inLine = true;
car.go_custom(FAST, SLOW);
}
else if (!lineLeft && lineCenter && lineRight) // 011
{
/**
* Lệch trái ít ... rẽ phải ... bánh trái quay nhanh hơn
*/
direction = -1;
inLine = true;
car.go_custom(NORMAL, SLOW);
}
/* ----------------------------------------------------------------------- */
else if ((!lineLeft && lineCenter && !lineRight) || (lineLeft && lineCenter && lineRight)) // 010 hoặc 111
{
/**
* Giữa line ... đường line nhỏ ... chạy chậm
*/
direction = 0;
inLine = true;
car.go_forward(SLOW);
}
/* ----------------------------------------------------------------------- */
else if (lineLeft && lineCenter && !lineRight) // 110
{
/**
* Lệch phải ít ... rẽ trái ... bánh phải quay nhanh hơn
*/
direction = 1;
inLine = true;
car.go_custom(SLOW, NORMAL);
}
else if (lineLeft && !lineCenter && !lineRight) // 100
{
/**
* Lệch phải nhiều ... rẽ trái ... bánh phải quay nhanh hơn nữa
*/
direction = 2;
inLine = true;
car.go_custom(SLOW, FAST);
}
else if (!lineLeft && !lineCenter && !lineRight) // 000
{
/**
* Hết line, hoặc ngoài line
* Bắt đầu đếm thời gian "timeout"
* Kể từ lúc xe vừa lệch ra ngoài line
*/
if (inLine)
{
capturePoint = millis();
inLine = false;
}
/**
* Nếu quá thời gian giới hạn
* Sẽ cho xe dừng lại
*/
if (millis() - capturePoint >= TIME_OUT)
{
direction = 0;
inLine = false;
car.stop();
}
/**
* Nếu vẫn còn trong thời gian cho phép
* Điều khiển xe cố gắng tìm lại line
* Dựa theo giá trị lần cuối lệch line
*/
else
{
switch (direction)
{
case 2: // Lệch phải nhiều ... xoay trái vừa
car.rotate_left(NORMAL);
break;
case 1: // Lệch phải ít ... xoay trái chậm
car.rotate_left(SLOW);
break;
case 0: // Giữa ... cứ đi thẳng tiếp
car.go_forward(SLOW);
break;
case -1: // Lệch trái ít ... xoay phải chậm
car.rotate_right(SLOW);
break;
case -2: // Lệch trái nhiều ... xoay phải vừa
car.rotate_right(NORMAL);
break;
}
}
}
}
/* ------------------------------------------------------------------------- */
/* RUN ONE TIME */
/* ------------------------------------------------------------------------- */
void setup()
{
// Thiết lập các chân đọc cảm biến
pinMode(PIN_OUT1, INPUT);
pinMode(PIN_OUT2, INPUT);
pinMode(PIN_OUT3, INPUT);
// Tránh xe chạy liền ngay khi vừa khởi động
delay(TIME_OUT);
}
/* ------------------------------------------------------------------------- */
/* MAIN */
/* ------------------------------------------------------------------------- */
void loop()
{
// Đọc giá trị từ cảm biến
read_sensor();
// Từ giá trị cảm biến dò line
// Tính toán mức độ lệch so với line
// Và điều khiển xe để dò lại line
level_line();
}
Sau đó kết nối cáp cho bo Uno với máy tính, rồi chọn đúng Board "Arduino Uno" và Port COM.
|
Chú ý: Các bạn nhớ tắt nguồn pin trên xe trước khi nạp chương trình. |
Bấm nút Upload (Ctrl+U) và đợi cho chương trình được nạp xong.
Giải thích code.
Trong mỗi đoạn code trên mình có kèm theo ghi chú giải thích code.
Đặc biệt, các bạn cần chú ý đến những ghi chú ( //! ).
Đây là những dòng #define, để chỉnh các thông số cấu hình cho code.
|
... Trong đó có 4 thông số bạn có thể điều chỉnh:
|
Chương trình hoạt động:
Khi mới khởi động:
- Nếu xe không thấy đường line nào, sẽ đứng yên.
- Và ngược lại, xe sẽ di chuyển ngay lập tức để bám theo đường line.
Đầu tiên, chương trình dùng hàm read_sensor() để đọc các giá trị Digital trả về từ 3 cảm biến dò line.
Dựa theo giá trị phản hồi về từ cảm biến, chương trình dùng hàm level_line() để dự đoán tình huống hiện tại như là:
- Đang di chuyển ngay đúng giữa đường line.
- Đang lệch trái ít hay nhiều.
- Đang lệch phải ít hay nhiều.
- Hoặc đang ngoài đường line.
Dựa theo mỗi trường hợp, sẽ có cách điều khiển xe khác nhau, như tiếp tục duy trì đi thẳng đều, hay rẽ trái - rẽ phải để bù độ lệch.
Đặc biệt, nếu đang di chuyển mà đầu xe bị mất line. Chỉ có 2 khả năng xảy ra:
- Một là xe đã đi hết đường line.
- Hai là xe di chuyển lệch ra ngoài đường line.
Lúc này, xe sẽ có khoảng thời gian TIME_OUT để cố gắng tìm lại đường line. Nếu sau khi hết khoảng thời gian này, xe vẫn không dò thấy line thì sẽ dừng lại.
Thao tác điều khiển.
Sau khi nạp code xong, đặt xe trước vạch bắt đầu đường line. Bật nguồn pin và xem kết quả.
Video hướng dẫn.
... link video youtube