THIẾT kế ROBOT tự HÀNH AGV

CHƯƠNG 1: TỔNG QUAN VỀ ROBOT AGV....................................................7 1.1. LỊCH SỬ NGHIÊN CỨU.................................................................................7 1.1.1. Lịch sử phát triển của xe AGV...............................................................7 1.1.2. Phân loại xe AGV...................................................................................8 1.2. MỤC TIÊU CỦA ĐỀ TÀI..............................................................................11 1.3. PHƯƠNG PHÁP THỰC HIỆN ĐỀ TÀI........................................................11 CHƯƠNG 2: MÔ HÌNH ĐỘNG HỌC, ĐỘNG LỰC HỌC MOBILE ROBOT 3 BÁNH..........................................................................................................................13 2.1. TÍNH TOÁN ĐỘNG HỌC...............................................................................13 2.2.TÍNH TOÁN ĐỘNG LỰC HỌC.......................................................................16 CHƯƠNG 3: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN.................................................20 3.1.BỘ ĐIỀU KHIỂN TRUNG TÂM ARDUINO UNO R3..............................20 3.2.MẠCH CẦU H ĐIỀU KHIỂN TỐC ĐỘ ĐỘNG CƠ L298........................................22 3.4.BỘ NGUỒN......................................................................................................23 CHƯƠNG 4: ĐỀ XUẤT MÔ HÌNH XE AGV............................................................29 4.1. KHUNG VÀ ĐỘNG CƠ...............................................................................29 4.2. VỎ CỦA ROBOT.....................................................................................................31 4.3. ROBOT HOÀN THÀNH...................................................................34 1

lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN TRƯỜNG CAO ĐẲNG NGHỀ ĐÀ NẴNG KHOA TỰ ĐỘNG HÓA ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: THIẾT KẾ ROBOT TỰ HÀNH AGV GVHD: Trần Ngọc Tân SVTH: Nguyễn Quý Phi Nguyễn Ngọc Thành Lê Duy Tân Đặng Trung Kiên Lớp: 20CĐT1B Khóa: 2020 - 2023 Đà Nẵng /2022 ` Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN LỜI CẢM ƠN Trong thời gian làm đồ án tốt nghiệp, em nhận nhiều giúp đỡ, đóng góp ý kiến bảo nhiệt tình thầy Em xin gửi lời cảm ơn chân thành đến thầy Trần Ngọc Tân, giảng viên môn - trường Cao đẳng nghề Đà Nẵng, người tận tình hướng dẫn, bảo chúng em suốt trình làm đồ án Em xin chân thành cảm ơn thầy cô giáo trường Cao đẳng nghề Đà Nẵng nói chung, thầy Khoa Tự Động Hóa nói riêng dạy dỗ cho em kiến thức môn đại cương môn chuyên ngành, giúp em có sở lý thuyết vững vàng tạo điều kiện giúp đỡ em suốt trình học tập Cuối cùng, em xin chân thành cảm ơn gia đình bạn bè, ln tạo điều kiện, quan tâm, giúp đỡ, động viên em suốt trình học tập hồn thành khố luận tốt nghiệp ` Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN MỤC LỤC Lời nói đầu …… …… …… …… …… …… …… …… ………………………… CHƯƠNG 1: TỔNG QUAN VỀ ROBOT AGV 1.1 LỊCH SỬ NGHIÊN CỨU .7 1.1.1 Lịch sử phát triển xe AGV .7 1.1.2 Phân loại xe AGV 1.2 MỤC TIÊU CỦA ĐỀ TÀI 11 1.3 PHƯƠNG PHÁP THỰC HIỆN ĐỀ TÀI 11 CHƯƠNG 2: MƠ HÌNH ĐỘNG HỌC, ĐỘNG LỰC HỌC MOBILE ROBOT BÁNH 13 2.1 TÍNH TỐN ĐỘNG HỌC .13 2.2.TÍNH TỐN ĐỘNG LỰC HỌC .16 CHƯƠNG 3: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN .20 3.1.BỘ ĐIỀU KHIỂN TRUNG TÂM ARDUINO UNO R3 20 3.2.MẠCH CẦU H ĐIỀU KHIỂN TỐC ĐỘ ĐỘNG CƠ L298 22 3.4.BỘ NGUỒN 23 CHƯƠNG 4: ĐỀ XUẤT MƠ HÌNH XE AGV 29 4.1 KHUNG VÀ ĐỘNG CƠ .29 4.2 VỎ CỦA ROBOT 31 4.3 ROBOT HOÀN THÀNH 34 ` Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN MỤC LỤC HÌNH Hình 1: Sơ đồ vận hành xe AGV nhà máy Hình 2: Xe AGV kéo hàng .9 Hình 3: Xe AGV chở hàng .9 Hình 4: Xe AGV nâng hàng 10 Y Hình 1: Hệ tọa độ Robot .13 Hình 2: Mơ hình phân tích lực bánh sau robot [6] 16 Y Hình 1: Arduino Mega 2560 20 Hình 2: Mạch cầu H dung IR2184 22 Hình 3: pin 16 Hình 5: Pin Lipo 25 Hình 6: Cảm biến dị line LED 25 Hình 7: Module dị line 26 Hình 8: Module đọc ADC 26 Hình 9: Cảm biến vật cản hồng ngoại DS30c4 28 Hình 1: Module cụm bánh động lực .29 Hình 2: Module cụm nâng .32 Hình 3:Khung mơ hình AGV 34 ` Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN BẢNG KÝ HIỆU VIẾT TẮT ` Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN LỜI NÓI ĐẦU Cùng với phát tiển ngành khoa học kỹ thuật việc ứng dụng cơng nghệ kỹ thuật vào công nghiệp ngày trở nên quan trọng phổ biến Việc nghiên cứu, phát triển mơ hình Robot tự hành AGV nhằm mục đích thay quy trình vận hành thủ cơng phương pháp tự động hóa, góp phần nâng cao suất lao động, giảm chi phí đem lại hiệu to lớn sản xuất cơng nghiệp Bài báo cịn đề cập tới việc tính tốn lựa chọn thiết thiết bị xây dựng mơ hình Robot tự hành AGV Nghiên cứu xuất phát từ nhu cầu thực tiễn góp phần tăng xuất công ty Xu nghiên cứu phù hợp với phát triển công nghiệp nước nhà, đặc biệt thuận lợi cho việc nghiên cứu sinh viên Along with the development of the science and technology, the application of technical technologies in the industry has become increasingly important and popular The research and development to design AGV robot model aims to replace manual previous operation processes automation method, contributing, enhance improving labor productivity, reducing costs and bringing about great efficiency in industrial production The article also refer to the calculation and the selection of equipment for building AGV robot models The research is proposed from the practical needs of the workers, contributing to increasing productivity The trend of the topic is in line with the development of the domestic industry, especially, convenient for the study of students Nhóm sinh viên thực : Nguyễn Quý Phi Nguyễn Ngọc Lê Duy Tân ` Đặng Trung Kiên Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN CHƯƠNG 1: TỔNG QUAN VỀ MOBILE ROBOT LỊCH SỬ NGHIÊN CỨU Lịch sử phát triển xe AGV Robot đời từ kỷ trước công nguyên Chu Mục Vương (Trung Quốc) người Yan Shi phác thảo ý tưởng máy tự động khí Đến năm 1927, robot điện tử lần xuất phim ảnh Năm 1948, nguyên lý tảng robot tự động hóa đời, tạo tiền đề cho robot học sau Cho tới năm 1961, Mobile Robot điện tử giới đời, đặt tên Unimate.Trong loại Mobile Robot áp dụng lĩnh vực cơng nghiệp, Robot tự hành mặt đất AGV( Autonomous Guided Vehicles) giới quan tâm đến Hệ thống xe tự hành (AGV) tồn từ năm 1953 Barrett Electronics Of Northbrook, bang Illinois – USA, Savant Automation of Walker, bang Michigan – USA Một nhà phát minh sáng chế phương pháp tự động hoá người xe tải kéo mà sử dụng nhà máy nhiều năm nhờ vào giấc mơ Robot tự hành AGV chế tạo để vận chuyển phôi gia công vào năm 70, vấn đề định hướng xe tự hành vấn đề quan trọng Ban đầu AGV xe kéo nhỏ chạy theo đường dẫn Công nghệ năm 70 điều khiển hệ thống để nâng cao khả tính linh hoạt xe AGV, xe không dùng để kéo, đẩy hàng kho, mà cịn có chức trung gian , kết nối trình sản xuất, lắp ráp, phân loại hàng hóa Trải qua nhiều năm, cơng nghệ phát triển mạnh hơn, q trình tự hành, AGV lập trình để giao tiếp với robot khác nhằm đảm bảo sản phẩm chuyển qua trạm, kho nơi mà sau chúng giữ lại chuyển đến vị trí khác ` Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP Phân loại xe AGV Xe AGV sử dụng với mục đích chung để chuyển hàng nhà máy, kho chứa sản phẩm GVHD : TRẦN NGỌC TÂN Hình 1: Sơ đồ vận hành xe AGV nhà máy Ngày nay, xe AGV có nhiều dịng sản phẩm khác thị trường Các sản phẩm AGV bao gồm: Các sản phẩm AGV bao gồm: ➢ Xe kéo (Towing Vehicle) Xe kéo xuất cá c dòng xe AGV thịnh hành thị trường Loại kéo nhiều toa hàng khác trở từ 8000 đến 60000 pounds Ưu điểm hệ thống xe kéo: • Khả chun chở lớn • Có thể dự đốn lên kế hoạch tính hiệu việc chuyên chở việc đảm bảo an tồn • Tính an tồn tố Hình 2: Xe AGV kéo hàng ➢ Xe chở ( Unit Load Vehicle) Xe chở trang bị tâng khay chứa nâng, hạ chuyênr động băng tải, đai xích Ưu điểm xe chở: • Tải trọng phân phối di chuyển theo yêu cầu • Thời gian đáp ứng nhanh gọn ` Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP • • • GVHD : TRẦN NGỌC TÂN Giảm hư hại tài sản Đường linh hoạt Giảm thiếu tắc nghẽn giao thơng chun chở Hình 3: Xe AGV chở hàng ` Trang Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN Xe đẩy ( Cart Vehicle) Xe đẩy cho có tính linh hoạt rẻ tiền Chúng sử dụng để chuyên chở vật liệu hệ thống lắp ráp ➢ Xe nâng ( Fork Vehicle) Có khả nâng tải trọng sàn bục cao hay khối hàng đặt giá ➢ Hình 4: Xe AGV nâng hàng AGV khơng theo đường dẫn ( free path navigation) • Có thể di chuyển đến vị trí khơng gian làm việc Đây loại xe AGV có tính linh hoạt cao định vị nhờ cảm biến quay hồi chuyển (Gyroscop sensor) để xác định hướng di chuyển, cảm biến laser để xác định vị trí vật thể xung quanh trog trình di chuyển, hệ thống định vị cục (Local navigation Location) để xác định tọa đọ tức thời, việc thiết kế loại xe địi hỏi cơng nghệ cao phức tạp xe AGV khác • Loại chạy theo đường dẫn ( fixed path navigation) Xe AGV loại thiết kế chạy theo đường dẫn định sẵn bao gồm loại đường sau: 10 ` Trang 10 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN CHƯƠNG 3: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN Trong q trình tìm hiểu nghiên cứu, nhóm đồ án biết ưu, nhược nhiều loại thiết bị khác từ đưa lựa chọn thiết bị phù hợp cho đồ án Dưới thiết bị đó: 3.1.BỘ ĐIỀU KHIỂN TRUNG TÂM ARDUINO MEGA 2560 Arduino Uno R3 loại bo mạch vi điều khiển dựa ATmega328P Nó bao gồm tồn thứ cần thiết để giữ vi điều khiển cần gắn vào PC với trợ giúp cáp USB cung cấp nguồn điện chuyển đổi AC-DC pin để bắt đầu Thuật ngữ Uno có nghĩa “một” ngôn ngữ “Ý” chọn để đánh dấu việc phát hành phần mềm Arduino’s IDE 1.0 R3 Arduino Uno sửa đổi thứ gần Arduino Uno Bảng Arduino phần mềm IDE phiên tham chiếu Arduino phát hành Bảng mạch Uno bảng chuỗi USB- Bảng Arduino mơ hình tham chiếu thiết kế cho tảng Arduino Hình 1: Arduino uno r3 Các tính bật Arduino uno r3: Arduino uno đọc tín hiệu analog tín hiệu số từ cảm biến khác nhau, đầu tín hiệu để điều khiển bơm ( thơng qua mạch cơng suất ), bật tắt bóng led, kết nối lên cloud nhiều tính khác Arduino boards dễ dàng thay đổi chức cách lập trình lại thơng qua trình biên dịch arduino IDE (uploading software) Khơng giống board vi điều khiển khác, Arduino không cần đến ` Trang 21 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN mạch nạp riêng biệt bên để nạp lại chương trình, Arduino cần đến cáp USB Arduino IDE sử dụng ngơn ngữ lập trình C/C++ bản, ngôn ngữ phổ biến nên giúp bạn tiếp cận với Arduino dễ dàng Arduino boards có nhiều phiên bản, khác số lượng đầu vào ( input, output ) tốc độ, điện áp hoạt động , hình dáng … Tuy nhiên chúng lập trình thơng qua arduino IDE nên câu lệnh hồn tồn giống Đây điều thuận lợi sử dụng Arduino Thông số kỹ thuật Arduino uno r3 Một số lưu ý sử dụng Arduino uno r3 Khi bắt đầu sử dụng Arduino Uno R3, bạn nên ý lựa chọn lại board Bằng cách vào Tool → Board → Arduino Uno R3 Nhằm tránh trước bạn sử dụng loại Arduino khác cổng nhận board cũ nên build bạn gặp lỗi Khi sử dụng chân RX, TX cuả Arduino, bạn nên nhớ rút dây cắm chân bắt đầu upload Sau cắm lại bình thường sử dụng để tránh gặp phải lỗi Không phép cắm trực tiếp chân GND vào chân nguồn 5V, dẫn tới hỏng mạch ` Trang 22 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN 3.2.MẠCH CẦU H ĐIỀU KHIỂN TỐC ĐỘ ĐỘNG CƠ Hình 2: Mạch cầu H dung L298 Đặc tính kỹ thuật: • Điện áp hoạt động: 5~30VDC • Cơng suất tối đa: 25W cầu (lưu ý công suất = dòng điện x điện áp nên áp cấp vào cao, dịng nhỏ, cơng suất có định 25W) • Dịng tối đa cho cầu H là: 2A • Mức điện áp logic: Low -0.3V~1.5V, High: 2.3V~Vss • Kích thước: 43x43x27mm • Hai chân cấp nguồn trực tiếp đến động 12V power 5V power Ngoài bạn cấp nguồn -12V 12V • Power GND GDN nguồn cấp cho động • Jump A enable B enable • chân Input gồm có: IN1, IN2, IN3,IN4 • Output A nối với động A Khi nối bạn phải ý ký hiệu “ +;-” bạn nối ngược động chạy theo chiều ngược • Gồm hai phần điều khiển động Có thể điều khiển cho động dây động dây ` Trang 23 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN 3.4.BỘ NGUỒN ➢ Nguồn cấp cho động Với đề tài này, nhóm đồ án sử dụng viên pin 3,7V nối tiếp với tạo thành nguồn 7,4V để cấp nguồn cho động trình hoạt động Hình 4: Pin 18650 Thơng số kỹ thuật: • Loại SP : Pin Lithium Ion sạc (Li-Ion) • Mã SP: Panasonic NCR18650B • Dung lượng : 3400mAh • Dòng xả : 10A • Size : 18650 • Kích thước khoảng : 18 mm x 65 mm/ viên • Điện thế : 3.7V, sạc đầy có thể đạt đến 4.2v • HSX: Panasonic • Xuất xứ : Japan ` Trang 24 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN 3.5.HỆ THỐNG DỊ ĐƯỜNG Module dị line được dùng nhiều thi robot robocon, đua xe dò line, robo sumo Sử dụng cho robot tự động bán tự động Ngay cả cơng nghiệp dùng tính dị line cho robot tự hành Hình 6: Cảm biến dò line led Bộ sản phẩm dị line gồm có : Thanh cảm biến dị đường BFD-1000 được thiết kế dùng để phát line đen line trắng Trên cảm biến có cảm biến hồng ngoại hướng xuống đất nhằm phát line, cảm biến hồng ngoại đặt phía trước với cơng tắc hành trình báo hiệu đã đụng vật Tín hiệu ngõ dạng số dễ dàng cho việc xử lý Hình 7: Module dò line Lưu ý: sử dụng module cần setup khoảng cách từ mặt quang trở đến mặt sàn 5mm10mm để tối ưu độ xác quang trở Hướng dẫn sử dụng: ➢ Set up module: ➢ Đấu Bus bo quang trở led vào bo main có kí hiệu ADC sensor ➢ Đấu bus bo main có ki hiệu OUT vđk bo master ➢ Cấp nguồn vào thấy led đơn chớp lần (dấu hiệu bo tốt) ➢ Sau gắn vào robot tiến hành canh chỉnh lấy thơng số sân sau: Đưa phần bo quang trở tới vùng sáng sân thường mầu đỏ xanh (không phải vạch trắng) vùng sân ➢ b Nhấn nút mode thấy led đơn chạy từ ngồi vào ➢ c Bạn nhấn nút nút , nút cho ta độ nhạy khác nhau, nút nhạy nút nhạy ➢ d Sau nhân nút xong đưa vào vạch trắng sân để quan sát, chưa tốt ` Trang 25 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN đưa sân (không phải vạch trắng) để chụp lại nút khác, lại thử đưa vào vạch trắng để xem kết có ok khơng, ok nhấn nút mode để khỏi chương trình setup lưu kết lại vào eeprom thấy led đơn chạy từ Ngoài chế độ cịn nhạy bạn tổ hợp nút mode + nút nút mode + nút để vào chế độ setup độ nhạy modul giảm bớt đi, sau bước thao tác trên, dấu hiệu phân biệt chế độ : • mode nhấn xong khơng có led sáng • mode + nút nhấn tổ hợp có led sáng • mode + nút nhấn tổ hợp có led sáng ➢ Tín hiệu trả MCU: Các chân tín hiệu từ -> module chuyển đổi tương ứng với kênh từ -> module dị line Khi kênh khơng phát line trắng chân tương ứng mức (5V) Ngược lại kênh phát line trắng chân tương ứng với kênh kéo xuống mức (0V) ` Trang 26 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN 3.6.HỆ THỐNG TRÁNH VẬT CẢN Hình 9: Cảm Biến Vật Cản Hồng Ngoại E18-D80NK Thông số kỹ thuật: • Nguồn điện cung cấp: 5VDC • Khoảng cách phát hiện: ~ 80cm • Có thể điều chỉnh khoảng cách qua biến trở • Dịng kích ngõ ra: 300mA • Ngõ dạng NPN cực thu hở giúp tùy biến điện áp ngõ ra, trở treo lên áp tạo thành điện áp ngõ nhiêu • Chất liệu sản phẩm: nhựa • Có led hiển thị ngõ màu đỏ • Kích thước: 1.8cm (D) x 7.0cm (L) ` Trang 27 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN CHƯƠNG 4: ĐỀ XUẤT MÔ HÌNH XE AGV 4.1 KHUNG XE VÀ CỤM ĐỘNG LỰC Hình 4.1 khung dộng xe 4.2 VỎ MƠ HÌNH ` Trang 28 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN Hình 4.2 vỏ xe 4.3 HỒN THÀNH hình 4.3 robot hồn thiện ` Trang 29 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN Hình 4.4 robot hồn thiện Hình 4.4 robot hồn thiện ` Trang 30 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN /*******************END******************/ Đoạn chương trình để xe tránh vật cản void MODE_2(void) { /* TIM5 Interrupts enable */ TIM_ITConfig(TIM5, 1, ENABLE); /* TIM5 enable counter */ GPIO_ResetBits(GPIOD,GPIO_Pin_15); } if (count>=10) { TIM_Cmd(TIM5, ENABLE); count=0; while (mode_interrupt==0) { if (TIM5_Flag==1) {TIM5_Flag=0; count++; if ((count%5)==1) { GPIO_SetBits(GPIOA, GPIO_Pin_5); GPIO_SetBits(GPIOD,GPIO_Pin_15); } else if ((count%5)==2) { ` Trang 31 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN GPIO_ResetBits(GPIOA, GPIO_Pi count=0; if (dis>40) { Round_R=300; Round_L=300; } else if (dis>20) { if (Counter_R>=Counter_L){Round_R=200;Round_L=100;} else {Round_R=100;Round_L=200;} } Else { if (Counter_R>=Counter_L){Round_R=200;Round_L=0;} else {Round_R=0;Round_L=200;} } } PID(); } } TIM_ITConfig(TIM5, 1, DISABLE); TIM_Cmd(TIM5, DISABLE); } \* * chương trình ngắt xảy có thay đổi xung Echo để tính độ rộng xung từ tính * khoảng cách *\ void EXTI4_IRQHandler(void) { if(EXTI_GetITStatus(EXTI_Line4) != RESET) { EXTI_ClearITPendingBit(EXTI_Line4); if (GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4)==1) { ` Trang 32 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN TIM_SetCounter (TIM9,0); /* TIM9 enable counter */ TIM_Cmd(TIM9, ENABLE); } Else { dis=TIM_GetCounter(TIM9)/87;///58/1.5; TIM_Cmd(TIM9, DISABLE); } } } Đoạn chương trình tính giải thuật PID void PID(void) { Counter_L= (TIM_GetCounter(TIM2)-20000); TIM_SetCounter (TIM2, 20000); Counter_R= (20000-TIM_GetCounter(TIM3)); TIM_SetCounter (TIM3, 20000); //***************PID dong co ben trai(trong so = 0)************* pre_pre_err[0]=pre_err[0]; pre_err[0]=err[0]; err[0]=Round_L-Counter_L; pre_u[0]=u[0]; u[0]=pre_u[0]+Kp*(err[0]pre_err[0])+Ki*(T/2)*(err[0]+pre_err[0])+(Kd/T)*(err[0]2*pre_err[0]+pre_pre_err[0]); //***************PID dong co ben phai(trong so = 1)************* pre_pre_err[1]=pre_err[1]; pre_err[1]=err[1]; err[1]=Round_R-Counter_R; pre_u[1]=u[1]; u[1]=pre_u[1]+Kp*(err[1]pre_err[1])+Ki*(T/2)*(err[1]+pre_err[1])+(Kd/T)*(err[1]2*pre_err[1]+pre_pre_err[1]); PWM3_duty=u[0]; if (PWM3_duty>667) PWM3_duty=667; else if (PWM3_duty667) PWM1_duty=667; else if (PWM1_dutyCCR1=PWM1_duty; TIM4->CCR3=PWM3_duty; } **************************END************************** ` Trang 34 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com) lOMoARcPSD|2935381 ĐỒ ÁN TỐT NGHIỆP GVHD : TRẦN NGỌC TÂN NHẬN XÉT ……………………………………………………………………………………… …………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ` Trang 35 Downloaded by EBOOKBKMT VMTC (nguyenphihung1009@gmail.com)