Professional Documents
Culture Documents
DATN-Tran Anh Tuan
DATN-Tran Anh Tuan
MỤC LỤC
MỤC LỤC ................................................................................................................. - 1 -
DANH MỤC CÁC BẢNG........................................................................................ - 3 -
DANH MỤC CÁC HÌNH ........................................................................................ - 4 -
PHẦN MỞ ĐẦU ....................................................................................................... - 5 -
CHƢƠNG 1: TỔNG QUAN ROBOT DÒ ĐƢỜNG ............................................ - 6 -
1.1 Tổng quan về ứng dụng vi điều khiển ............................................................... - 6 -
1.2 Giới thiệu về robot dò đường ............................................................................ - 6 -
1.2.1 Giới thiệu .................................................................................................... - 6 -
1.2.2 Hoạt động .................................................................................................... - 7 -
1.3 Ứng dụng robot dò đường ................................................................................. - 9 -
CHƢƠNG II: THIẾT KẾ VÀ TÌM HIỂU LINH KIỆN CỦA HỆ THỐNG ... - 10 -
2.1 Yêu cầu đề tài ................................................................................................ - 10 -
2.2 Giải pháp thiết kế .......................................................................................... - 10 -
2.2.1 Sơ đồ khối ................................................................................................. - 10 -
2.2.2 Phân tích chức năng các khối ................................................................... - 10 -
2.2.3 Nguyên lý hoạt động của hệ thống ........................................................... - 11 -
2.3 Lựa chọn linh kiện ........................................................................................ - 13 -
2.3.1 Khối điều khiển......................................................................................... - 13 -
2.3.2 Module điều khiển động cơ L293D .......................................................... - 18 -
2.3.3 Module cảm biến hông ngoại TCRT5000 ................................................ - 20 -
2.3.4 Động cơ giảm tốc DC ............................................................................... - 21 -
2.3.5 LED (Light Emitting Diode) .................................................................... - 21 -
2.3.6 Nguồn Adapter 12V2A ............................................................................. - 22 -
CHƢƠNG III: PHÂN TÍCH THUẬT TOÁN PID VÀ ỨNG DỤNG TRONG
ROBOT DÒ ĐƢỜNG............................................................................................. - 23 -
3.1 Giới thiệu....................................................................................................... - 23 -
3.2 Định nghĩa thuật toán PID .......................................................................... - 23 -
3.3 Phƣơng pháp điều chế độ rộng xung (PWM) ............................................. - 24 -
3.4 Giải thuật điều khiển vi tích phân tỉ lệ (PID) ............................................. - 25 -
3.5 Ứng dụng giải thuật trên robot dò đƣờng .................................................. - 26 -
3.5.1 Mô hình PID áp dụng cho robot dò đường ............................................... - 26 -
3.5.2 Giải thuật PID ........................................................................................... - 27 -
SVTH: Trần Tuấn Anh – CCVT15A002 Trang - 1 -
Thiết kế và thi công robot dò đường sử dụng thuật toán PID
PHẦN MỞ ĐẦU
Ngày nay, với những ứng dụng của khoa học kỹ thuật tiên tiến, thế giới của
chúng ta đã và đang ngày một thay đổi, văn minh và hiện đại hơn. Sự phát triển
của kỹ thuật điện tử đã tạo ra hàng loạt những thiết bị với các đặc điểm nổi bật
như sự chính xác cao, tốc độ nhanh, gọn nhẹ là những yếu tố rất cần thiết góp
phần cho hoạt động của con người đạt hiệu quả cao.
Các bộ điều khiển sử dụng vi điều khiển tuy đơn giản nhưng để vận hành
và sử dụng được lại là một điều rất phức tạp. Các bộ vi điều khiển theo thời gian
cùng với sự phát triển của công nghệ bán dẫn đã tiến triển rất nhanh, từ các bộ vi
điều khiển 4 bit đơn giản đến các bộ vi điều khiển 32 bit, rồi sau này là 64 bit.
Điện tử đang trở thành một ngành khoa học đa nhiệm vụ. Điện tử đã đáp ứng
được những đòi hỏi không ngừng từ các lĩnh vực công – nông – lâm – ngư
nghiệp cho đến các nhu cầu cần thiết trong hoạt động đời sống hằng ngày.
Robot có vai trò rất quan trọng trong đời sống hiện nay đặc biệt là những
ứng dụng của nó trong công nghiệp, sản xuất kinh tế, quốc phòng…. Vì vậy em
chọn đề tài “Thiết kế và thi công robot dò đường áp dụng thuật toán PID” để
làm đồ án tốt nghiệp.
Mặc dù đã rất cố gắng thiết kế và làm mạch nhưng do thời gian ngắn và
năng lực còn hạn chế nên mạch vẫn còn những sai sót. Em mong thầy (cô) giáo
góp ý để em sớm hoàn thành đồ án này.
Em xin chân thành cảm ơn!
muốn trên sân. Với các chế độ 1,2.. chiếc xe có thể thực hiện được đa dạng trong
việc di chuyển và có nhiều hướng phát triển thêm sau này.
1 2 3 4 5 6 7 8
Hình 1.4 Cách bố trí cảm biến
Dựa vào nững nhận biết vạch của cảm biến mà từ đó xe có thể rẽ trái, rẽ phải.
Để rẽ trái: cảm biến 5,6 có tính hiệu hoặc cảm biến 5,6,7 có tính hiệu.
Khi r trái động cơ trái chạy chậm lại động cơ phải chạy nhanh hơn. Khi
r trái ở ngã tư động cơ phải quay thuận,động cơ trái quay ngược.
Để rẽ phải: cảm biến 3,4 có tính hiệu hoặc cảm biến 2,3,4 có tính hiệu.
Khi r phải động cơ phải chạy chậm, động cơ trái chạy nhanh hơn. Khi r
phải ở ngã tư động cơ trái quay thuận, động cơ phải quay ngược.
Để chạy thẳng: hai cảm biến 4,5 có tính hiệu. Hai động cơ trái phải chạy
cùng tốc độ.
Nhận biết số ngã tư 1 hoặc 8 cảm biến đồng thời có tính hiệu.
1.3 Ứng dụng robot dò đƣờng
- Ứng dụng trong cuộc sống hằng ngày: chúng ta có thể chế tạo máy hút
bụi tự động từ một robot mini gắn một máy hút bụi nhỏ ở trên, robot sẽ tự động
di chuyển trong nhà đồng thời máy hút bụi cũng hoạt động sẽ làm sạch bụi bẩn
dưới mặt sàn nhà.
- Ứng dụng trong công nghiệp: chế tạo một robot vận chuyển hàng hóa
trên tuyến đường cố định, dựa vào khả năng di chuyển theo vạch của robot ta
vạch một đường đi từ xưởng sản xuất A sang xưởng sản xuất B từ đó robot co
thể chở hàng hóa đi theo đường đã vật từ xưởng sản xuất A sang xưởng sản xuất
B và ngược lại.
- Ứng dụng trong quân sự: chế tạo những robot dò mìn, hiện nay khoa học
đã phát triển hơn nhiều robot có khả năng di chuyển dựa vào việc xử lý ảnh do
robot chụp lại, từ đó robot có thể tìm được đường đi và phát hiện ra mìn,vật cần
tìm…
- Khối cảm biến: Sử dụng module cảm biến dò đường để nhận tín hiệu từ bên
ngoài xong chuyển tín hiệu sang khối so sánh.
- Khối so sánh: Nhận tín hiệu từ khối cảm biến, so sánh và khếch đại tín hiệu
để đưa ra mức logic 0 tương ứng với trạng thái có vạch và mức logic 1
tương ứng với trạng thái không vạch rồi chuyển tín hiệu cho khối điều
khiển trung tâm xử lý.
- Khối điều khiển trung tâm: Sử dụng Arduino Uno R3, nhận tín hiệu từ khối
so sánh rồi t nh toán để xác định trạng thái robot đang di chuyển và xuất
ra tín hiệu xung chuyển qua khối điều khiển động cơ.
- Khối điều khiển động cơ: Sử dụng module L298 để nhận tín hiệu từ khối
điều khiển trung tâm rồi xử lý tín hiệu để điều hướng xe di chuyển đúng.
2.2.3 Nguyên lý hoạt động của hệ thống
Tín hiệu từ cảm biến được đưa qua mạch so sánh và khếch đại tín hiệu để
đưa ra mức logic “0” ứng với trạng thái có vạch và mức logic “1” ứng với trạng
thái không có vạch. Dựa vào tín hiệu từ khối cảm biến, khối điều khiển trung
tâm sẽ t nh toán để xác định trạng thái robot đang di chuyển và xuất ra tín hiệu
xung PWM để điều khiển các động cơ th ng qua mạch điều khiển động cơ sử
dụng IC L298. Việc đó sẽ giúp robot tự di chuyển theo quỹ đạo xác định trước.
Điều này giúp cho robot có khả năng dò đường một cách chính xác và
hiệu quả.
Hiện nay, thị trường có rất nhiều loại cảm biến khác nhau. Chúng giúp cho robot
có khả năng dò đường một cách chính xác và hiệu quả như, cảm biến la bàn điện
từ, cảm biến tiếp xúc, cảm biến quang, bộ giải mã encoder, hệ thống định vị toàn
cầu GPS, camera quan sát kết hợp công nghệ xử lý ảnh… Trong phạm vi bài
báo này, tác giả sử dụng các cặp cảm biến quang được đặt cạnh nhau theo hàng
ngang dưới thân của robot, vì robot thực nghiệm di chuyển theo vạch k màu
đen trên nền màu trắng.
Nguyên lý hoạt động của mạch cảm biến thu phát quang dựa trên sự hấp thụ
và phản xạ ánh sáng của các màu sắc khác nhau của nền và đường đi. Vạch màu
trắng có khả năng phản xạ ánh sáng tốt hơn vạch màu đen. Khi đó, quang trở sẽ
nhận được các tia sáng phản xạ có cường độ lớn làm cho giá trị điện trở giảm
khá nhiều, dẫn đến điện áp trên quang trở là Vmin sẽ thấp. Ngược lại, vạch màu
đen có khả năng phản xạ ánh sáng kém hơn vạch màu trắng. Khi đó, quang trở
nhận được các tia sáng phản xạ có cường độ thấp làm cho giá trị điện trở của
quang trở giảm kh ng đáng kể, dẫn đến điện áp trên quang trở lúc này là Vmax sẽ
cao.
Hình 3.b thể hiện sơ đồ mạch nguyên lý của cảm biến. Điện áp trên biến trở Vref,
được tính theo công thức (1), dùng để so sánh với điện áp từ quang trở để
chuyển đổi thành các mức logic 0 tương ứng với 0Vdc hoặc mức logic 1 tương
ứng với 5Vdc mà vi điều khiển có thể hiểu được.
Arduino UNO.
dùng các thiết bị sử dụng những nguồn điện riêng biệt thì những chân này phải
được nối với nhau.
5V: cấp điện áp 5V đầu ra. Dòng tối đa cho phép ở chân này là 500mA.
3.3V: cấp điện áp 3.3V đầu ra. Dòng tối đa cho phép ở chân này là 50mA.
Vin (Voltage Input): để cấp nguồn ngoài cho Arduino UNO, bạn nối cực
dương của nguồn với chân này và cực âm của nguồn với chân GND.
IOREF: điện áp hoạt động của vi điều khiển trên Arduino UNO có thể
được đo ở chân này. Và dĩ nhiên nó lu n là 5V. Mặc dù vậy kh ng được lấy
nguồn 5V từ chân này để sử dụng bởi chức năng của nó không phải là cấp
nguồn.
RESET: việc nhấn nút Reset trên board để reset vi điều khiển tương
đương với việc chân RESET được nối với GND qua 1 điện trở 10KΩ.
Các cổng vào/ra: Arduino UNO có 14 chân digital dùng để đọc hoặc
xuất tín hiệu. Chúng chỉ có 2 mức điện áp là 0V và 5V với dòng vào/ra tối đa
trên mỗi chân là 40mA. Ở mỗi chân đều có các điện trở pull-up từ được cài đặt
ngay trong vi điều khiển Atmega328 (mặc định thì các điện trở này kh ng được
kết nối). Một số chân digital có các chức năng đặc biệt như sau:
2 chân Serial: 0 (RX) và 1 (TX): dùng để gửi (transmit – TX) và nhận
(receive – RX) dữ liệu TTL Serial. Arduino Uno có thể giao tiếp với thiết bị
khác thông qua 2 chân này. Kết nối bluetooth thường thấy nói nôm na chính là
kết nối Serial không dây. Nếu không cần giao tiếp Serial, không nên sử dụng 2
chân này nếu không cần thiết
Chân PWM (~): 3, 5, 6, 9, 10, và 11: cho phép bạn xuất ra xung PWM
với độ phân giải 8bit (giá trị từ 0 → 28-1 tương ứng với 0V → 5V) bằng hàm
analogWrite(). Nói một cách đơn giản, có thể điều chỉnh được điện áp ra ở chân
này từ mức 0V đến 5V thay vì chỉ cố định ở mức 0V và 5V như những chân
khác.
Chân giao tiếp SPI: 10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK). Ngoài
các chức năng th ng thường, 4 chân này còn dùng để truyền phát dữ liệu bằng
giao thức SPI với các thiết bị khác.
LED 13: trên Arduino UNO có 1 đèn led màu cam (k hiệu chữ L). Khi
bấm nút Reset, sẽ thấy đèn này nhấp nháy để báo hiệu. Nó được nối với chân số
13. Khi chân này được người dùng sử dụng, LED sẽ sáng.
Arduino UNO có 6 chân analog (A0 → A5) cung cấp độ phân giải tín
hiệu 10bit (0 → 210-1) để đọc giá trị điện áp trong khoảng 0V → 5V. Với chân
AREF trên board, có thể để đưa vào điện áp tham chiếu khi sử dụng các chân
analog. Tức là nếu cấp điện áp 2.5V vào chân này thì có thể dùng các chân
analog để đo điện áp trong khoảng từ 0V → 2.5V với độ phân giải vẫn là 10bit.
Đặc biệt, Arduino UNO có 2 chân A4 (SDA) và A5 (SCL) hỗ trợ giao
tiếp I2C/TWI với các thiết bị khác.
Atmega328 là một ch p vi điều khiển được sản xuất bời hãng Atmel thuộc
họ MegaAVR có sức mạnh hơn hẳn Atmega8. Atmega 328 là một bộ vi điều
khiển 8 bít dựa trên kiến trúc RISC bộ nhớ chương trình 32KB ISP flash có thể
ghi xóa hàng nghìn lần, 1KB EEPROM, một bộ nhớ RAM vô cùng lớn trong thế
giới vi xử lý 8 bít (2KB SRAM)
Tổng chập của ba tác động này dùng để điều chỉnh quá trình thông qua
một phần tử điều khiển như vị trí của van điều khiển hay bộ nguồn của phần tử
gia nhiệt. Nhờ vậy, những giá trị này có thể làm sáng tỏ về quan hệ thời
gian: (P) phụ thuộc vào sai số hiện tại, (I) phụ thuộc vào t ch lũy các sai số quá
khứ, và (D) dự đoán các sai số tương lai, dựa vào tốc độ thay đổi hiện tại.
3.3 Phƣơng pháp điều chế độ rộng xung (PWM)
Tốc độ quay của động cơ một chiều tỉ lệ thuận với điện áp đầu vào. Do
đó, cách đơn giản nhất để điều khiển tốc độ quay của rotor là thay đổi mức điện
áp đặt vào động cơ.
Nguyên tắc cơ bản để thay đổi tốc độ quay động cơ của phương pháp
PWM là giữ nguyên giá trị điện áp vào và thay đổi thời gian đặt điện áp vào
động cơ. Điều này có nghĩa, thời gian mức cao Ton trong một chu kỳ của xung
ngõ vào động cơ càng lớn thì điện áp trung bình đặt vào động cơ càng cao,
ngược lại thời gian mức thấp Toff trong một chu kỳ của xung ngõ vào động cơ
càng lớn điện áp trung bình đặt vào động cơ càng thấp (hình 3.1).
Đại lượng mô tả mối quan hệ giữa khoảng thời gian Ton và Toff được gọi là độ
rộng xung (duty_cycle), được tính theo công thức:
(a) Giản đồ độ rộng xung (b) Giản đồ xung với theo duty_cycle
Hình 3.1 Giản đồ thời gian của xung PWM
Thí dụ: Trong hình 3.1 (b), nếu chu kỳ xung 1kHz; thời gian xung mức cao Ton
là 0.3ms và thời gian xung mức thấp Toff là 0.7ms. Biên độ xung là 12Vdc. Tốc
độ quay của rotor là 1500 vòng/phút. Theo công thức: duty_cycle = 30% và điện
áp trung bình đặt vào động cơ là 12 x 30% = 3.6Vdc. Tốc độ quay của rotor là
- Hàm lỗi e(t) tại thời điểm t: độ lệch giữa giá trị mong muốn và
giá trị đo được:
e(t) = Vset - Vmo(t)
Trong đó:
Vset : Setpoint Value (giá trị điện áp mong muốn).
Vmo (t) : Measurable Output Value (giá trị điện áp đo được
tại thời điểm t).
Proportional P(t) (hàm tỉ lệ): điều khiển tỉ lệ với giá trị lỗi hiện tại.
P(t) = Kp.e(t)
Trong đó:
Kp: hằng số tỉ lệ, tham số dùng để tinh chỉnh.
Integral I(t) (hàm t ch phân): điều khiển tương ứng với mức lỗi được
t ch lũy theo thời gian.
Trong đó:
Ki: hằng số điều chỉnh hàm t ch phân.
Derivative D(t) (hàm vi phân): điều khiển tương ứng với tốc độ
thay đổi của lỗi, hay độ dốc lỗi theo thời gian.
Trong đó:
Kd: hằng số điều chỉnh hàm vi phân.
3.5 Ứng dụng giải thuật trên robot dò đƣờng
3.5.1 Mô hình PID áp dụng cho robot dò đường
Giải thuật PID được tích hợp vào trong giải thuật điều khiển robot dò
đường để tính toán và xuất ra các giá trị PWM điều khiển 2 động cơ gắn với 2
bánh sau của robot dựa vào độ sai lệch giữa giá trị đo đạc ngõ vào và giá trị
mong muốn. Độ sai lệch, hay còn gọi là giá trị lỗi, được tính toán trong giải
thuật PID là độ lệch giữa trạng thái hiện tại của robot so với đường đi. Có nghĩa
là, độ sai lệch giữa giá trị hiện tại của bộ cảm biến so với giá trị của cảm biến
trong trường hợp robot chạy thẳng về ph a trước. Việc áp dụng giải thuật PID
trên robot dò đường sử dụng cảm biến quang được trình bày bằng một sơ đồ
khối như hình 3.3.
Hình 3.3 Sơ đồ khối ứng dụng giải thuật PID vào robot dò đường
Trước khi đề cập ứng dụng giải thuật PID vào robot dò đường, tác giả đưa
ra các định nghĩa của các thuật ngữ liên quan.
Vị trí mục tiêu Vset: trung tâm của đường đi, vị trí mà ta muốn robot
hướng đến.
Vị trí thực tại Vmo(t): vị trí thực của robot, có thể lệch bên trái hoặc lệch
bên phải so với đường đi của robot.
Lỗi e(t): độ sai lệch giữa vị trí mục tiêu và vị trí thực của robot, có thể là
giá trị âm hoặc dương hoặc bằng 0, được tính theo công thức.
Hệ số Kp, Ki, Kd là các hằng số tương ứng được sử dụng để điều chỉnh
ảnh hưởng của các khâu tỉ lệ, khâu tích phân và khâu vi phân.
3.5.2 Giải thuật PID
Giải thuật PID là một chuỗi các phép toán để xác định các giá trị của khâu
điều khiển tỉ lệ, khâu điều khiển vi phân và khâu điều khiển t ch phân. Sau đó,
kết hợp các giá trị lại với nhau và xuất ra giá trị điều rộng xung PWM để điều
khiển 2 động cơ bánh sau của robot. Khâu điều khiển tỉ lệ t nh toán độ lệch của
robot, xác định được robot đang lệch phải hay lệch trái đường và xuất ra các giá
trị PWM để điều khiển hai động cơ giúp robot quay về vị trí mục tiêu.
Độ lệch : e(n) = Vmo(n) – Vset
Đáp ứng khâu tỉ lệ : P(n) = Kp . e(n)
Trong đó : n là số lần lấy mẫu
Hằng số Kp càng lớn thì sự điều chỉnh càng lớn khi lỗi càng lớn.
Ngược lại, Kp càng nhỏ thì sẽ đáp ứng điều chỉnh nhỏ đối với lỗi.
Khâu điều khiển tích phân tính toán dựa trên sai số t ch lũy theo thời
gian. Robot càng lệch xa đường thì sai số t ch lũy càng lớn. Khâu này thể
hiện rằng trong quá khứ robot có bám đường đi tốt hay không.
Sai số tích lũy:
E(n) e(n)
n1
Đáp ứng khâu t ch phân: I(n)= Ki . E(n) (11)
Trong đó : n là số lần lấy mẫu
Khâu vi phân tính toán dựa trên sai số hiện tại. Khâu này cho biết tốc
độ robot dao động qua lại của đường đi.
Tốc độ dao động: ∆e(n) = e(n) – e(n-1) (12)
Đáp ứng khâu vi phân: D(n) = Kd . ∆e(n) (13)
Cuối cùng, giá trị PWM để xuất ra điều khiển cho các động cơ điều
khiển hoạt động của robot sẽ là
PWM = P(n) + I(n) + D(n) = Kp . e(n) + Ki . I(n) + Kd . D(n)
PWM = Kp . [Vmo(n) – Vset] + Ki .e(n)+ Kd . [e(n) – e(n-1)]
n1
3.5.3 Hiệu chỉnh thuật toán PID
Khi sử dụng giải thuật PID để điều khiển robot dò đường, kết quả ban đầu
cho thấy robot hoạt động kh ng như mong đợi. Trường hợp này được lý giải là
do các giá trị của các hệ số Kp, Ki và Kd chưa phù hợp. Mỗi robot có một đặc
tính vật lý khác nhau do đó các hệ số này cũng phải thay đổi để phù hợp. Phần
tiếp theo, tác giả đề xuất các bước thực nghiệm đơn giản để xác định các hệ số
Kp, Ki và Kd.
Bao gồm các nút lệnh menu (File, Edit, Sketch, Tools, Help). Ph a dưới là
các icon cho phép sử dụng nhanh các chức năng thường dùng của IDE được
miêu tả như sau:
Chúng ta có thể tìm thấy một vài hướng dẫn khắc phục các lỗi thường gặp
khi lập trình Arduino tại Lỗi của Arduino? Và các lỗi thường gặp khi lập trình
Arduino
Khi lập trình, cần chọn port (cổng kết nối khi gắn board vào) và board
(tên board mà bạn sử dụng). Giả sử, đang dùng mạch Arduino Uno, và khi gắn
board này vào máy tính bằng cáp USB nó được nhận là COM4 thì bạn chỉnh
như thế này là có thể lập trình được.
Hình 4.2 Tấm formex dùng làm tấm nền cho robot
4.2.2 Hệ thống đường line cho robot
Sử dụng băng keo cách nhiệt để làm đường line cho robot vì độ bám dính
tốt, sự đàn hồi của băng keo tốt, k ch thước phù hợp với k ch thước để làm
đường line cho robot.
Phần tiếp theo là áp dụng phương pháp PWM kết hợp với giải thuật PID,
còn gọi là PWM vòng kín, vào việc thực nghiệm mô hình robot thật. Robot được
thiết kế và chế tạo sử dụng dãy 5 cặp cảm biến thu phát quang để dò đường, mỗi
cặp cảm biến cách nhau 25mm. Vì, đường đi là vạch đen được k trên nền trắng
và có bề rộng 30mm.
PHỤ LỤC
#include <AFMotor.h>
#include <PID_v1.h>
AF_DCMotor motor1(3);
AF_DCMotor motor2(4);
//float Kp=17,Ki=0,Kd=13;
double error=0,PID_value,Setpoint;
//int initial_motor_speed=100;
float lech=0;
void read_sensor_values(void);
void calculate_pid(void);
void motor_control(void);
int initial_motor_speed,dem;
void setup()
Serial.println("Motor test!");
// turn on motor
Setpoint=5;
motor1.setSpeed(255);
motor1.run(RELEASE);
motor2.setSpeed(255);
motor2.run(RELEASE);
myPID.SetMode(AUTOMATIC);
initial_motor_speed = 120;
void loop()
read_sensor_values();
calculate_pid();
motor_control();
// Serial.println(PID_value);
// Serial.println(lech);
void read_sensor_values()
sensor[0]=digitalRead(A0);
//sensor[0]=0;
sensor[1]=digitalRead(A1);
sensor[2]=digitalRead(A2);
sensor[3]=digitalRead(A3);
sensor[4]=digitalRead(A4);
//sensor[4]=0;
if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(se
nsor[4]==1))
lech=1;
error=1;
else
if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(se
nsor[4]==1))
lech=1;
error=2;
else
if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(se
nsor[4]==0))
lech=1;
error=3;
else
if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==1)&&(se
nsor[4]==0))
lech=1;
error=3,5;
else
if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==0)&&(se
nsor[4]==0))
error=5;
else
if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==1)&&(sensor[3]==0)&&(se
nsor[4]==0))
lech=2;
error=3,5;
else
if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(se
nsor[4]==0))
lech=2;
error=3;
else
if((sensor[0]==1)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(se
nsor[4]==0))
lech=2;
error=2;
else
if((sensor[0]==1)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(se
nsor[4]==0))
lech=2;
error=1;
else
if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(se
nsor[4]==0))
//dem++;
error=0;
void calculate_pid()
myPID.Compute();
void motor_control()
if(left_motor_speed<0)left_motor_speed=0;
if(right_motor_speed<0)right_motor_speed=0;
if(left_motor_speed>255)left_motor_speed=255;
if(right_motor_speed>255)right_motor_speed=255;
// Serial.println(left_motor_speed);
// printf
// The motor speed should not exceed the max PWM value
Serial.println(dem);
// Serial.println(left_motor_speed);
// printf
// The motor speed should not exceed the max PWM valuez
if(lech==1){
else if(lech==2)
motor1.run(FORWARD);
motor2.run(FORWARD);