CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN
Trên hình 4.14 ta thấy khi cảm biến dịch chuyển sang phải một đoạn 26-d thì luơn luơn cĩ 2 led nằm trong đường line, và do đĩ tín hiệu analog đo đƣợc sẽ như nhau, tương tự khi cảm biến di chuyển sang trái đoạn 2d-26 thì chỉ cĩ 1 led nằm trong đường line. Ta phải chọn giá trị của d sao cho các khoảng cách này là nhỏ nhất.
𝑒 = min (max(2𝑑 − 23,26 − 𝑑))
d = 17 mm
e = 9 mm
Vậy ta chọn d = 17 mm.
4.1.8 Lựa chọn vi điều khiển trung tâm.
Với bộ điều khiển tập trung được lựa chọn, vi điều khiển phải đảm bảo thực hiện được các tác vụ sau:
Điều khiển tồn bộ hoạt động của robot: Đọc tín hiệu ADC từ cảm biến dị đường, tín hiệu từ cảm biến vật cản, điều khiển tốc độ 2 động cơ, điều khiển cơ cấu nâng hạ thơng qua điều khiển chiều 1 xylanh điện, giao tiếp truyền nhận dữ liệu với đầu đọc thẻ RFID thơng qua chuẩn SPI, mơ đun WIFI ESP32 thơng qua chuẩn truyền UART.
Yêu cầu:
+ Cĩ ít nhất 7 chân đọc ADC từ cảm biến, 1 chân đọc ADC từ Loadcell. + Ít nhất 3 chân xung PWM và 4 chân IO điều khiển chiều động cơ. + Hỗ trợ giao tiếp SPI với RC522, giao tiếp UART với ESP32.
+ 6 chân IO đọc tín hiệu từ cảm biến vật cản, 1 IO đọc tín hiệu từ cơng tắc hành trình xylanh điện.
+ Đọc tín hiệu hồi tiếp encoder từ 2 động cơ. Lựa chọn:
Vi điều khiển Tiva C với chip TM4C123GH6PM của hãng Texas Instruments được lựa chọn với các thơng số sau:
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN
- Cĩ 12 kênh chuyển đổi ADC.
- 5 bộ timer/counter 16/32 bit.
- 6 chân ngắt ngồi đọc encoder.
- Hỗ trợ giao tiếp SPI, I2C, UART.
Hình 4. 14: Vi điều khiển TIVA C của hãng TI [8]
4.1.9 Lựa chọn nguồn
Bảng 4. 4: Điệp áp – dịng điện các thiết bị được sử dụng
Thiết bị Số
lượng Dịng max Điện áp Tổng cộng
Động cơ 2 2500 mA 24V 5000 mA – 24V
Động cơ nâng 1 1500 mA 24V 1500 mA – 24V
Cảm biến dị line 1 40 mA 5V 40 mA – 5V
Tiva C 1 100mA 5V 100 mA – 5V
Driver 3 200mA 24V 600mA-24V
RC522 1 26mA 3.3V 26mA - 3.3V
ESP32 1 90mA 3.3V 90mA - 3.3V
Cảm biến vật cản 6 100mA 5V 600mA – 5V
Cảm biến
Loadcell 1 150mA 5V 150mA – 5V
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN
Vậy nguồn điện cung cấp cần cĩ cơng suất tối thiểu:
Chọn bình Acquy SB2450 LITHIUM ION BATTERY với thơng số:
- Điện áp: 25.6 V - Dịng: 50Ah - Số lần nạp xả: 3000-5000 4.2. Sơ đồ kết nối hệ thống Đường tín hiệu Nguồn cung cấp 740 mA – 5V 106 mA – 3.3V
Sensor 1 Sensor 2 Sensor 3 Sensor 4 Sensor 5 Sensor 6 Sensor 7
`
RFID TIVA C
ESP32 WIFI
` DC
Encoder DC Xy lanh dẫn hướng
3.3V ASM1117 LM2596 24v 5V Nguồn Driver Xylanh Driver Motor 2 Driver Motor 1 Vậ t cản 1 Vậ t cản 2 Vậ t cản 3 Vậ t cản 4 Vậ t cản 5 Vậ t c ản 6
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN
ESP32
- Nguồn 3.3VDC cung cấp cho mơ đun RFID và mơ đun WIFI ESP32, sử dụng
mạch giảm áp ASM1117 chuyển từ nguồn 5V=> 3.3V.
- Nguồn 5VDC cung cấp cho encoder, nút nhấn, cảm biến dị đường, cảm biến vật
cản, vi điều khiển Tiva C, cảm biến loadcell, sử dụng mạch giảm áp LM2596 chuyển từ điện áp 24VDC => 5 VDC với dịng điện cực đại là 3A.
- Nguồn 24VDC cung cấp cho 3 driver động cơ.
Sơ đồ kết nối nguồn và các thiết bị hình 4.8 thể hiện nguyên lý hoạt động tổng thể của mạch điều khiển, việc đấu nối giữa các chân của vi điều khiển đối với các thiết bị sẽ được giới thiệu rõ hơn ở các phần tiếp theo.
Kết nối mạch và thiết lập thơng số mạch giao tiếp dữ liệu giữa ESP32 và
Tiva C
Hình 4. 16: Sơ đồ kết nối giữa ESP32 và Tiva C
Sơ đồ hình 4.9 thể hiện quan hệ truyền nhận dữ liệu giữa tiva C và ESP32 thơng qua giao tiếp UART. Để đảm bảo q trình truyền nhận dữ liệu chính xác, thiết lập các thơng số truyền nhận dữ liệu mặc định cho cả 2 thiết bị tương thích với nhau. Thiết lập tốc độ Baud 115200 và khung truyền 8 bits dữ liệu, khơng sử dụng parity, 1 bit stop.
Thiết lập các thơng số của mơ-đun ESP32 thơng qua tập lệnh AT như sau: Bảng 4. 5: Tập lệnh AT thiết lập cho ESP32
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN
Lệnh AT Giải thích
AT+CWMODE=3 ESP32 được thiết lập trong vai
trị máy khách (Client)
AT+CWJAP="SSID","password" ESP32 tự động kết nối vào mạng
khi cung cấp đúng tên WIFI và mật khẩu
AT+CIPSTART="TCP","192.168.101.110",8080 ESP32 kết nối vào máy chủ cĩ địa chỉ IP và Port được nhập, truyền thơng bằng giao thức TCP
ESP32 được thiết lập thơng qua TIVA C để tự động kết nối vào server máy tính thơng qua ứng dụng được viết bằng ngơn ngữ C++, lúc này máy tính đĩng vai trị là máy chủ (Server). Sau quá trình kết nối thành cơng, Tiva C và máy tính đã cĩ thể giao tiếp thơng qua sĩng WIFI, mơ đun ESP32 lúc này đĩng vai trị là trung gian trong quá trình truyền nhận dữ liệu.
Hình 4. 17: Mơ hình truyền nhận dữ liệu bằng ESP32
Máy tính truyền nhận dữ liệu với ESP32 thơng qua giao thức TCP, trong khi đĩ ở tầng dưới, ESP32 truyền nhận dữ liệu và nhận lệnh từ TIVA C thơng qua giao tiếp UART.
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN
Hình 4. 18: Sơ đồ kết nối RC522 và Tiva C
Sơ đồ hình 4.10 thể hiện quan hệ truyền nhận dữ liệu giữa tiva C và ESP32 thơng qua giao tiếp SPI. Mơ-đun RC522 cĩ nhiệm vụ đọc mã thẻ RFID được gá dưới mặt sàn, tại các điểm giao nhau của giao lộ, từ đĩ hệ thống sẽ quản lí được vị trí hiện tại của AGV.
Kết nối mạch điều khiển hai bánh xe
Để điều khiển hai động cơ DC servo thơng qua driver DCS3T-28 ta cần kết nối 1 chân IO để điều khiển chiều, 1 chân PWM để điều khiển tốc độ động cơ. Mỗi driver cho phép điều khiển tốc độ một động cơ.
Hình 4. 19: Sơ đồ kết nối mạch điều khiển động cơ Kết nối mạch điều khiển xylanh điện. Kết nối mạch điều khiển xylanh điện.
Vin Vin SDA A3 SCK A4 RC522 MOSI A5 MISO A6 TIVA C RST A7 GND GND PWM DIR DIR
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN RC5 L_PWM M1 R_EN RB5 RA2 L_EN M2 XYLANH ĐIỆN R_PWM RC7 GND BTS7960 Vin GND TIVA C Vin
Để điều khiển xylanh điện, mạch BTS7960 được sử dụng, từ Tiva C, kết nối 2 chân IO vào 2 chân LPWM và RPWM trên BTS7960 để điều khiển chiều, nối chung 2 chân R_EN và L_EN từ driver vào 1 chân PWM của Tiva C.
Do xylanh điện thuộc dạng điều khiển vịng hở (khơng cĩ tín hiệu hồi tiếp từ ENCODER) nên cơng tắc hành trình cận dưới được sử dụng. Khi xylanh nâng đủ chiều cao yêu cầu, cơng tắc hành trình trả tín hiệu về cho vi điều khiển để kết thúc quá trình nâng tải.
Hình 4. 20: Sơ đồ kết nối mạch điều khiển xylanh
Kết nối mạch cảm biến dị đường
Mạch cảm biến dị đường sử dụng cảm biến Photo-Transistor được kết nối với các chân đọc tín hiệu ADC của vi điều khiển Tiva C.
S1 E1 S2 E2 S3 E3 S4 E0 S5 D1 S6 D2 S7 E5 CƠNG TẮC HÀNH TRÌNH TIVA C
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN E + DAT A PE 4 A + A- HX711 SCK TIVA C
Hình 4. 21: Sơ đồ kết nối cảm biến dị line
Kết nối cảm biến khối lượng Loadcell
Cảm biến khối lượng loadcell được kết nối với kit Tiva C thơng qua mơ-đun ADC HX711. Phương pháp kết nối được trình bày ở hình 4.22.
Hình 4. 22: Sơ đồ kết nối Loadcell
4.3. Thiết kế mạch in bằng phần mềm Altium Designer
Nhằm đảm bảo robot hoạt động ổn định, mạch in được thiết kế bằng phần mềm Altium Designer được thiết kế nhằm hạn chế đi dây điện bằng phương pháp truyền thống, đồng thời đảm bảo kết nối các thiết bị chính xác hơn.
Mạch cảm biến dị line
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỆN
Hình 4. 24: Mạch PCB cảm biến dị line Mạch kết nối các thiết bị Mạch kết nối các thiết bị
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
CHƯƠNG 5: THIẾT KẾ BỘ ĐIỀU KHIỂN
Với kết quả đạt được ở chương 4, cấu trúc phần cứng của thiết bị đã cơ bản được hồn thành, kết cấu cơ khí và hệ thống điện đã được thiết kế. Trong chương này, sẽ tập trung thiết kế bộ điều khiển cho robot bao gồm: thiết kế ứng dụng điều khiển bằng ngơn ngữ C++, thiết kế bộ điều khiển PID cho động cơ với chế độ tải thay đổi, giải thuật xác định vị trí line sử dụng cảm biến Photo-Transistor.
5.1. Thiết kế ứng dụng điều khiển bằng ngơn ngữ C++.
Như phần trước đã trình bày, AGV và máy tính sẽ giao tiếp được với nhau khi đủ các điều kiện sau:
- ESP32 và máy tính cùng kết nối chung một mạng WIFI
- Thiết lập ESP32 bằng lệnh AT như bảng 4.4
- Máy tính phải tạo được một Server với địa chỉ IP và Port, cung cấp địa chỉ IP và
Port cho ESP32 để mơ-đun này tự động kết nối vào với vai trị là Client.
Yêu cầu thiết kế ứng dụng:
- Cĩ khả năng thiết lập một Server và cho phép các Client kết nối vào.
- Cĩ khả năng truyền – nhận dữ liệu thơng qua giao thức TCP
Phần mềm Visual Studio được sử dụng để thiết kế giao diện thơng qua ngơn ngữ C++. Ứng dụng được thiết kế cho phép tạo một Server và các thiết bị cĩ thể kết nối vào, giao thức TCP được sử dụng để truyền nhận dữ liệu. Trên ứng dụng hiển thị bản đồ mơ phỏng hoạt động của AGV trong nhà kho, cho phép hiển thị tốc độ hai bánh và sai số dị line. Ngồi ra cịn được thiết kế các nút nhấn và các chế độ cho phép điều khiển robot bằng tay hoặc tự động.
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Hình 5. 1: Giao diện ứng dụng điều khiển AGV bằng máy tính Ở mục Connection, yêu cầu nhập 2 thơng số đĩ là: Ở mục Connection, yêu cầu nhập 2 thơng số đĩ là:
- Server: Địa chỉ IP của máy tính trong mạng đang kết nối (kiểm tra IP thơng qua
máy tính).
- Port: Cổng giao tiếp đăng kí mở (lựa chọn tùy ý).
Sau khi nhập 2 thơng số trên và nhấn nút Connect, một Server đã được mở ở cho phép các Client kết nối và giao tiếp.
Cĩ ba chế độ được đề nghị bao gồm: Auto Mode, Manual Mode và Lift Cargo, tương ứng với chế độ tự động, bằng tay và nâng hạ kệ hàng.
Trong chế độ Auto Mode: Sau khi nhập 3 điểm yêu cầu gồm Start Node, End Node và Cargo Node, robot sẽ tự động xác định đường đi ngắn nhất thơng qua giải thuật tìm đường ngắn nhất ASTAR.
Trong chế độ Manual Mode: Robot được điều khiển di chuyển bám line thẳng, quay trái, quay phải 900.
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN Bắt đầu Nhập địa chỉ IP Nhập địa chỉ PORT Nhấn Connect S Đ Robot đã kết nối S Đ Hiển thị đã kết nối Hiển thị sai số bám line, vận tốc 2 bánh, vị trí hiện tại
Gửi danh sách các điểm tìm được từ giải thuật
ASTAR Đọc tín hiệu từ Robot gửi lên Đ Đ Chọn Auto Nhập Start Node, Cargo Node, End
Node S Chọn Manual Đ Nhấn Đ Move S S S Đ Chọn Stop Nhấn Up Đ S Đ Kết thúc Down Nhấn Gửi: Cylinder_Do wn S Đ
Nhấn Stop Gửi: Stop Gửi: Cylinder_Up Gửi: Move Giải thuật ASTAR tìm được ngắn nhất
Chế độ Lift Cargo: Cho phép người sử dụng điều khiển cơ cấu nâng hạ, nâng lên, hạ xuống, dừng ở vị trí bất kì.
Lưu đồ giải thuật:
Hình 5. 2: Lưu đồ giải thuật chương trình điều khiển trên máy tính Chương trình điều khiển trên máy tính được viết bằng ngơn ngữ C++, cĩ chức Chương trình điều khiển trên máy tính được viết bằng ngơn ngữ C++, cĩ chức năng điều khiển, giám sát hoạt động của robot. Khi bắt đầu chạy chương trình, u cầu nhập địa chỉ IP của máy tính đang kết nối mạng, thiết lập cổng PORT, nhấn nút
Connect. Lúc này máy chủ đang ở chế độ chờ, đợi robot kết nối. Khi robot kết nối, thơng báo kết nối được hiển thị, lúc này người sử dụng lựa chọn các chức năng điều
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN ENCODER DC SERVO MOTOR DCS3T-28 PID Controller u(t)
khiển robot như đã trình bày ở trên. Trong quá trình robot di chuyển, các sai số dị line, vận tốc 2 bánh, vị trí hiện tại của robot được gửi lên liên tục.
5.2. Thiết kế bộ điều khiển PID cho chế độ tải trọng thay đổi.
Trong quá trình hoạt động của thiết bị tự hành, với nhiệm vụ nâng và di chuyển kệ hàng với khối lượng dao động từ 0 – 50kg, bài tốn đặt ra đĩ là đáp ứng của 2 động cơ trong trường hợp tải trọng thay đổi. Để giải quyết bài tốn trên, giải thuật PID được sử dụng kết hợp với cảm biến Loadcell để xác định khối lượng của kệ hàng, ứng với mỗi chế độ tải trọng nhất định, sẽ cĩ một bộ số PID thích hợp được lựa chọn từ thực nghiệm, nhằm luơn đảm bảo đáp ứng của động cơ nằm trong khoảng yêu cầu.
Khối lượng tải xác định từ
Loadcell
y(t)
Hình 5. 3: Mơ hình tổng quát hệ thống điều khiển Yêu cầu thiết kế bộ điều khiển PID: Yêu cầu thiết kế bộ điều khiển PID:
- Thời gian đáp ứng 0.1s.
- Độ vọt lố 20%
5.2.1. Bộ điều khiển PID chế độ khơng tải
Lựa chọn hệ số PID
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Tiến hành khảo sát tốc độ động cơ và điện áp cấp vơ ở chế độ khơng tải, sử dụng Driver DCS3T cho kết quả như sau:
Hình 5. 4: Đáp ứng 2 động cơ ở chế độ khơng tải Từ kết quả khảo sát cho thấy: Từ kết quả khảo sát cho thấy:
- Quan hệ giữa tốc độ động cơ và xung PWM là tuyến tính với cả 2 động cơ
- Hệ số gĩc của đường đáp ứng 2 động cơ được xấp xỉ là 3.18
Tiến hành khảo sát đáp ứng của động cơ trái ở chế độ khơng tải, PWM =55%
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN Hệ thống cĩ dạng bậc nhất:
Hình 5. 6: Độ thị đường cong đáp ứng của hệ bậc nhất Từ đồ thị, hàm truyền động cơ trái: Từ đồ thị, hàm truyền động cơ trái:
𝐺(𝑠) =
3.18
0,23𝑠 + 1 Sử dụng PID Tuner của Matlab để tìm bộ PID:
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Tiến hành hiệu chỉnh các hệ số qua thực nghiệm, ta tìm được các hệ số: KP = 0.29, KI = 5.36, KD = 0.00153
Đáp ứng của động cơ trái:
- Settling time: Ts= 0.071s
- Overshoot: M = 12.7%
Tiến hành tương tự cho động cơ phải, hàm truyền tìm được:
𝐺(𝑠) = 0,15𝑠+ 1 3.18
Tiến hành hiệu chỉnh các hệ số qua thực nghiệm, ta tìm được các hệ số: KP = 0.14, KI = 6.21, KD = 0.00235
Đáp ứng của động cơ trái:
- Settling time: Ts= 0,098s
- Overshoot: M = 15,7%
5.2.2. Bộ điều khiển PID ở chế độ cĩ tải.
Tiến hành khảo sát đáp ứng của 2 động cơ ở chế độ cĩ tải 10kg:
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN Từ kết quả khảo sát cho thấy:
- Quan hệ giữa tốc độ động cơ và xung PWM là tuyến tính với cả 2 động cơ