(Đồ án hcmute) lập trình robot 3 bánh di chuyển theo người sử dụng camera kinect

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP.HCM KHOA ĐÀO TẠO CHẤT LƯỢNG CAO NGÀNH CNKT ĐIỆN TỬ TRUYỀN THÔNG ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: LẬP TRÌNH ROBOT BÁNH DI CHUYỂN THEO NGƯỜI SỬ DỤNG CAMERA KINECT GVHD SVTH MSSV SVTH MSSV Khoá : ThS NGUYỄN TẤN NHƯ : VÕ NGỌC THẨM : 13141608 : NGUYỄN HỮU MINH ANH : 13141005 : 2013-2017 SKL 0 9 Tp Hồ Chí Minh, tháng 07/2017 an TRƢỜNG ĐẠI HỌC SƢ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƢỢNG CAO ĐỒ ÁN TỐT NGHIỆP LẬP TRÌNH ROBOT BÁNH DI CHUYỂN THEO NGƢỜI SỬ DỤNG CAMERA KINECT SVTH1: VÕ NGỌC THẨM MSSV : 13141608 SVTH2: NGUYỄN HỮU MINH ANH MSSV : 13141005 Khóa : 2013 – 2017 Ngành : Công Nghệ Kỹ Thuật Điện Tử Truyền Thông GVHD :Ths NGUYỄN TẤN NHƢ Tp Hồ Chí Minh, tháng năm 2017 i an CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc *** Tp Hồ Chí Minh, ngày 11 tháng năm 2017 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Võ Ngọc Thẩm Nguyễn Hữu Minh Anh MSSV: 13141608 MSSV: 13141005 Ngành:Công Nghệ Kỹ Thuật Điện tử, Truyền thông Lớp: 13141CLDT2 Giảng viên hƣớng dẫn: Nguyễn Tấn Nhƣ ĐT:0983483098 Ngày nhâ ̣n đề tài: 21/02/2017 Ngày nộp đề tài: 11/7/2017 Tên đề tài: Lập trình Robot bánh di chuyển theo ngƣời sử dụng camera Kinect Các số liệu, tài liệu ban đầu: Bên cạnh kiến thức môn Mạch điện, Điện tử bản,Vi xử lý Lập trình nhúng Đồ án đƣợc thực sở nghiên cứu tham khảo đồ án tốt nghiệp, cơng trình nghiên cứu khoa học lĩnh vực điều khiển Robot ứng dụng thực tế, ứng dụng lợi ích camera Kinect hỗ trợ phong phú hệ điều hành mã nguồn mở ROS việc kết hợp linh kiện để tạo Robot có chức di chuyển theo ngƣời nhằm ứng dụng vào thực tiễn sống Nội dung thƣ̣c hiê ̣n đề tài: - Tìm hiều động servo DC - Tìm hiều board arduno DUE - Tìm hiểu camera Kinect ii an - Tìm hiều phần mềm viết chƣơng trình cho Arduino DUE - Tìm hiểu phần mềm ROS - Tìm hiều thuật tốn PID - Thiết kế mạch điện bên - Viết chƣơng trình xử lí cho board Arduino - Cho robot chạy thực nghiệm - Điều chỉnh cho robot hoạt động ổn định - Viết báo cáo đồ án tốt nghiệp - Báo cáo đề tài tốt nghiệp Sản phẩm: Một Robot bánh di chuyển theo ngƣời sử dụng camera Kinect TRƢỞNG NGÀNH GIẢNG VIÊN HƢỚNG DẪN iii an CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh Phúc ******* PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƢỚNG DẪN Họ tên Sinh viên: Võ Ngọc Thẩm MSSV: 13141608 Nguyễn Hữu Minh Anh MSSV: 13141005 Ngành: Công Nghệ Kỹ Thuật Điện Tử, Truyền Thông Tên đề tài: Lập trình Robot bánh di chuyển theo ngƣời sử dụng camera Kinect Họ tên Giáo viên hƣớng dẫn: Nguyễn Tấn Nhƣ NHẬN XÉT Về nội dung đề tài & khối lƣợng thực hiện: ……………………………………………………………………………………… ………… …………………………………………………………………………… ……….……………………………………………………………………………… Ƣu điểm: ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… Khuyết điểm: ……………………………………………………………………………………… ……………………………………………………………………………………… …… …………………………………………………………………………… Đề nghị cho bảo vệ hay không? ……………………………………………………………………………………… Đánh giá loại: ……………………………………………………………………………………… Điểm:………………………… (Bằngchữ:…………………………………… ) ……………………………………………………………………………………… Tp Hồ Chí Minh, ngày tháng năm 2017 Giáo viên hƣớng dẫn (Ký & ghi rõ họ tên) iv an CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh Phúc ******* PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN Họ tên Sinh viên: Võ Ngọc Thẩm MSSV: 13141608 Nguyễn Hữu Minh Anh MSSV: 13141005 Ngành: Công Nghệ Kỹ Thuật Điện Tử, Truyền Thông Tên đề tài: Lập trình Robot bánh di chuyển theo ngƣời sử dụng camera Kinect Họ tên Giáo viên phản biện: ………………………………………………… NHẬN XÉT Về nội dung đề tài & khối lƣợng thực hiện: ……………………………………………………………………………………… ………… …………………………………………………………………………… ……….……………………………………………………………………………… Ƣu điểm: ……………………………………………………………………………………… ……………………………………………………………………………………… …………………………………………………………………………………… Khuyết điểm: ……………………………………………………………………………………… ……………………………………………………………………………………… …… ………………………………………………………………………… Đề nghị cho bảo vệ hay không? …………………………………………………………………………………… Đánh giá loại: ……………………………………………………………………………………… Điểm……………………… (Bằng chữ:…………………………………………) ……………………………………………………………………………………… Tp Hồ Chí Minh, ngày tháng năm 2017 Giáo viên phản biện (Ký & ghi rõ họ tên) v an LỜI CẢM ƠN Lời đầu tiên, sinh viên thực đề tài xin đƣợc phép chân thành gửi lời cảm ơn đến thầy giáo Nguyễn Tấn Nhƣ, giảng viên hƣớng dẫn đề tài định hƣớng trao đổi kinh nghiệm quý báu để sinh viên thực nội dung đề tài cách hoàn chỉnh Sinh viên thực đề tài xin trân trọng cảm ơn thầy(cô) trƣờng Đại Học Sƣ Phạm Kỹ Thuật nói chung thầy(cơ) mơn Điện Tử Cơng Nghiệp nói riêng tận tình truyền đạt kiến thức tảng để sinh viên thực hoàn thành tốt đề tài Sau xin cảm ơn ngƣời thân, bạn bè giúp đỡ để đồ án đƣợc hoàn thành tốt đẹp Tuy nhiên kinh nghiệm thiết kế hệ thống vi điều khiển, hệ thống Robot, trình độ kỹ giải quết vấn đề phát sinh lúc thực hạn chế, nắm bắt thông tin chƣa kịp thời nên khơng tránh khỏi sai sót Ngƣời thực đề tài mong nhận đƣợc thơng cảm, góp ý dẫn thêm thầy cô bạn Em xin chân thành cảm ơn Tp Hồ Chí Minh, ngày Sinh viên thực Võ Ngọc ThẩmNguyễn Hữu Minh Anh thángnăm 2017 Sinh viên thực vi an TÓM TẮT Robot dần thay ngƣời làm công việc nặng nhọc môi trƣờng độc hại Việc tạo robot thông minh hoạt động hồn tồn tự động để giúp đỡ ngƣời sống ngày nhƣ nhà máy, xí nghiệp nhiệm vụ cấp thiết, giới bƣớc vào cách mạng công nghệ Với mục đích nghiên cứu thi cơng đƣợc sản phẩm có tính ứng dụng cao vào thực tiễn, nhóm định chọn đề tài “Lập trình Robot bánh di chuyển theo ngƣời sử dụng camera Kinect” nghiên cứu thi công đƣợc Robot có chức di chuyển theo ngƣời thay ngƣời bƣng bê, vận chuyển vật nặng nhọc Dựa sở thu thập liệu hình ảnh camera, qua xử lý để điều khiển cấu chấp hành bánh xe di chuyển Cụ thể, đề tài nhóm chúng em sử dụng camera Kinect với gói thƣ viện Openni_tracker hỗ trợ để đọc liệu toạ độ vị trí hơng ngƣời truyền xử lý trung tâm máy tính Tại máy tính tính tốn tốc độ di chuyển robot gửi liệu xuống board Arduino DUE để điều khiển cho bánh xe chuyển động tất trình đƣợc thực dựa việc ứng dụng hệ điều hành điều khiển Robot Robot Operating System (ROS) Sau tháng thực đề tài, nhóm thi cơng đƣợc Robot di chuyển hồn tồn tự động theo ngƣời hoạt động cách ổn định Đây điểm bật so với ứng dụng Robot điều khiển tay trƣớc Sau đƣợc cải tiến mặt khí đề tài có ý nghĩa lớn sống, sở để phát triển ứng dụng Robot nhằm hỗ trợ tối đa cho ngƣời vii an SUMMARY Humans are gradually replaced by robots, which hard work in hazardous environments Making intelligent robots which can completely automatic operate to help humans in daily life, even in factory is such a necessary mission, especially, when the whole world is approaching the new technological age With the purpose to research and make applied products to reality, our team make decision to choose project named “Programming wheels Robot following human using Kinect camera”, which is made to follow human, carry, transport heavy things for human Based on collecting image data using camera, processor controls wheels to move In detail, in this project, our team uses Kinect camera with Openni_tracker package to read co-ordinate data of human head then transmit to central processing unit The PC will compute moving velocity of the robot, then send data to Arduino DUE board to control the wheels to move and all the processes are implemented based on application operating system to control the Robot (ROS) After over months implementing this project, our team made a completely automatic operating Robot following human and it worked stably This project is more outstanding than the former projects After being improved mechanics aspect, this project is meaningful to our life, it is a base to develop Robot application in order to assist people ultimately viii an MỤC LỤC ĐỒ ÁN TỐT NGHIỆP .i NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP ii PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƢỚNG DẪN iv PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN v LỜI CẢM ƠN vi TÓM TẮT .vii SUMMARY viii MỤC LỤC ix DANH MỤC CÁC CHỮ VIẾT TẮT .xii DANH MỤC CÁC BẢNG BIỂU xiii DANH MỤC CÁC HÌNH ẢNH BIỂU ĐỒ xiv Chƣơng TỔNG QUAN 1.1 Lý chọn đề tài 1.2 Tình hình nghiên cứu 1.3 Mục tiêu 1.4 Nội dung nghiên cứu 1.5 Giới hạn 1.6 Phƣơng pháp nghiên cứu 1.7 Bố cục đồ án Chƣơng CƠ SỞ LÝ THUYẾT 2.1 Hệ điều hành Robot Operating System (ROS) 2.1.1 ROS gì? 2.1.2 Cấu trúc ROS 2.2 Phƣơng pháp chuyển ảnh từ 2D sang 3D cảm biến Kinect ix an Hình 5.25: Biểu đồ vận tốc người đứng bên trái Kinect Từ ba biểu đồ cho chúng thấy đƣợc ảnh hƣởng lớn Angular_Kinect đến tốc độ hai bánh xe Ở hình 5.23 hình đồ thị ngƣời đứng Kinect tƣơng ứng Argular_Kinect xấp xỉ tốc độ hai bánh xe Robot xấp xỉ lúc Robot chuyển động thẳng Ở hình 5.24 ngƣời đứng lệch bên phải ta thấy tốc độ bánh xe trái chuyển động nhanh tốc độ bánh xe phải lúc Robot rẽ phải hình 5.25 ngƣời lệch bên trái ta thấy tốc độ bánh xe phải nhanh tốc độ bánh xe trái lúc Robot rẽ trái - Xét ảnh hƣởng Kp_Angular đến thay đổi tốc độ bánh xe  Với Kp_Angular = Hình 5.26: Tốc độ bánh xe Kp_Angular 69 an  Với Kp_Angular = Hình 5.27: Tốc độ bánh xe Kp_Angular Từ hình 5.26, 5.27 cho thấy đƣợc thay đổi Kp_Angular ảnh hƣởng lớn đến đáp ứng tốc độ bánh xe Robot mà cụ thể Kp_Angular lớn tốc độ đáp ứng tốc độ hai bánh xe nhanh - Xét di chuyển ngƣời thay đổi theo thời gian để thấy đƣợc ảnh hƣởng Angular_Kinect Cụ thể ngƣời di chuyển liên tục từ trái qua phải tốc độ hai bánh xe thay đổi, ngƣời bên trái V_right lớn V_left lúc Robot xoay trái, di chuyển đến tốc độ hai bánh xe lúc Robot thẳng ngƣời di chuyển sang đến bên phải V_left lớn V_right lúc Robot xoay phải, thay đổi thể hình 5.28 70 an Hình 5.28: Sự thay đổi tốc độ bánh xe người di chuyển từ trái sang phải b Điều khiển Linear Robot - Sự thay đổi Linear_Kinect ảnh hƣởng đến thay đổi vận tốc bánh xe trái (V_left) bánh xe phải (V_right) Nếu mục a xét thay đổi Angular_Kinect thay đổi góc phần xét thay đổi Linear_Kinect thay đổi di chuyển thẳng Robot  Khi Linear_Kinect nhỏ: Khi Linear_Kinect nhỏ tức ngƣời di chuyển gần Robot hơn, lúc tốc độ hai bánh xe ngƣời đứng vị trí giới hạn (0,5m) Robot tốc độ hai bánh xe Robot đứng yên nhƣ hình 5.29 71 an Hình 5.29: Tốc độ bánh xe Linear_Kinect nhỏ  Khi Linear_Kinect lớn: Khi Linear_Kinect lớn tức ngƣời di chuyển xa Robot, theo phƣơng thẳng lúc tốc độ hai bánh xe lớn nhau, chuyển xa so với tầm nhìn Kinect Robot khơng cịn nhận thấy ngƣời Robot đứng lại, theo kết nhƣ hình 5.30 Hình 5.30: Tốc độ bánh xe Linear_Kinect lớn 72 an Để so sánh tốc độ hai bánh xe Linear_Kinect thay đổi có biểu đồ nhƣ sau: Hình 5.31: Biểu đồ thể tốc độ bánh xe người di chuyển lại gần Robot Hình 5.32: Biểu đồ thể tốc độ bánh xe người di chuyển xa Robot Từ hình 5.31, 5.32 cho thấy đƣợc Linear_Kinect thay đổi hai bánh xe Robot ln có tốc độ Đối với hình 5.32 đứng gần Robot với khoảng cách nhỏ 0,5m lúc tốc độ hai bánh xe Robot đứng yên Với hình 5.32 di chuyển xa Robot di chuyển tốc độ hai bánh xe tăng Từ nhận thấy 73 an Linear_Kinect điều khiển tốc độ bánh xe Robot dựa vào khoảng cách Robot ngƣời không giống nhƣ Angular_Kinect điều khiển tốc độ bánh xe dựa vào góc quay - Xét ảnh hƣởng Kp_Linear đến thay đổi tốc độ bánh xe  Khi Kp_Linear = 0.1 Hình 5.33: Tốc độ bánh xe Kp_Linear 0.1  Khi Kp_Linear = 0.5 Hình 5.34: Tốc độ bánh xe Kp_Linear 0.5 74 an Từ hình 5.33, 5.34 thấy đƣợc Kp_Linear ảnh hƣởng lớn đến thay đổi tốc độ hai bánh xe, Kp_Linear lớn thay đổi nhanh Ban đầu ta đứng yên tùy theo khoảng cách mà Robot chạy nhanh hay chậm, nhƣng di chuyển xa tốc độ hai bánh xe Robot bắt đầu tăng - Xét di chuyển ngƣời thay đổi theo thời gian để thấy đƣợc ảnh hƣởng Linear_Kinect Cụ thể ngƣời liên tục di chuyển lại gần xa Robot tốc độ hai bánh xe hai tốc độ tăng giảm tuyến tính theo thay đổi ngƣời, theo kết nhƣ hình 5.35 Hình 5.35: Sự thay đổi tuyến tính tốc độ bánh xe người di chuyển 5.2 Nhận xét tổng hợp Trong suốt gần bốn tháng nghiên cứu, nổ lực tìm hiểu tài liệu chun mơn, tài liệu tiếng Anh, tài liệu internet, với giúp đỡ, hƣớng dẫn tận tình Thầy Nguyễn Tấn Nhƣ, nhóm hồn thành đƣợc đề tài tốt nghiệp với mơ hình Robot di chuyển đƣợc theo ngƣời Với kết đƣợc phân tích phần thấy đƣợc Robot hoạt động ổn định theo yêu cầu đề tài Kết chạy thực tế đánh giá thực mơ phỏng, theo kết nhƣ hình 5.36 75 an Hình 5.36: Kết thực tế mô đề tài Bên cạnh ngƣời di chuyển phạm vi nhận dạng Camera Kinect Robot di chuyển theo ngƣời di chuyển tầm kiểm sốt Camera Kinect Robot đứng lại Với kết chứng tỏ Camera Kinect tracking đƣợc khung xƣơng ngƣời Ngồi ra, nhóm tìm hiểu đƣợc chức hoạt động số linh kiện có đề tài, lập trình ứng dụng đƣợc kit Arduino, tìm hiểu ứng dụng thành công hệ điều hành ROS vào điều khiển thực tế Tuy nhiên, kiến thức nhóm cịn hạn hẹp nguồn tài liệu chủ yếu từ nƣớc nên q trình hoạt động đề tài cịn hạn chế Robot chƣa hoạt động tốt bề mặt khơng phẳng, khối lƣợng Robot cịn nặng ảnh hƣởng đến việc di chuyển Chƣa khai thác hết chức Camera Kinect 76 an Chƣơng KẾT LUẬN VÀ HƢỚNG PHÁT TRIỂN 6.1 Kết luận Sau thời gian nghiên cứu, với cố gắng nổ lực thành viên nhóm, phân chia cơng việc hợp lí phối hợp nhịp nhàng hai thành viên nhóm kết đồ án hoàn thành thời gian qui định yêu cầu đặt đồ án Robot nhận diện di chuyển theo ngƣời hệ điều hành ROS Dù nhƣng kết dừng lại mức mang tính chất tảng chủ yếu dựa vào thƣ viện có sẵn cộng đồng ROS hỗ trợ Và qua đồ án nhóm học đƣợc nhiều kiến thức rút nhiều kinh nghiệm quý báu nhƣ cách phối hợp làm việc nhóm, cách bố trí thời gian hợp lí, hiểu rõ khó khăn cần giải bắt tay vào nghiên cứu khoa học để khơng cịn ngỡ ngàng tới thành viên nhóm tự bƣớc đƣờng chọn tƣơng lai 6.2 Hƣớng phát triển Với kiến thức chuyên môn thời gian có hạn đề tài hồn thành với yêu cầu đặt ban đầu, với đề tài mở rộng thêm nhƣ: - Hiện Robot chƣa có nhận biết ngƣời cố định ta phát triển thêm - Trên Camera Kinect có động chân đế ta tận dụng để lập trình cho Robot tự động điều chỉnh theo chiều cao ngƣời - Khối lƣợng Robot cịn nặng phải sử dụng laptop điều khiển thay Laptop Rasberry Pi để lập trình cho Robot Robot nhỏ gọn 77 an TÀI LIỆU THAM KHẢO Tiếng Việt [1]Nguyễn Hồng Đức Nguyễn Văn Đức(2012), “ Robot Tự Hành Tránh Vật Cản Sử Dụng Thiết Bị Kinect”, Đồ Án Tốt Nghiệp Đại Học, Trƣờng ĐH Bách Khoa Tp.Hcm.[2] Nguyễn Tấn Lập Nguyễn Văn Nho(2016), “Điều khiển robot từ xa dùng ROS”, Đồ Án Tốt Nghiệp Đại Học, Trƣờng ĐH Sƣ Phạm Kỹ Thuật Tp.HCM [3] Nhóm 11 Lớp K55M(2016), “Báo Cáo Tìm Hiểu Cảm Biến Kinect”, Trƣờng ĐH Công Nghệ Tiếng Anh [4] Lentin Joseph(2015), “Mastering ROS for Robotics Programming” 481 page [5] B Ilias, S.A Abdul Shukor, S Yaacob, A.H Adom and M.H Mohd Razali(2014), “A Nurse Following Robot With High Speed Kinect Sensor”, ARPN Journal of Engineering and Applied Sciences [6] Matteo Munaro, Filippo Basso and Emanuele Menegatti(2014), “Tracking people within groups with RGB-D data”, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems [7] J ă org St ă uckler and Sven Behnke(2014), Following Human Guidance to Cooperatively Carry a Large Object”, 2011 11th IEEE-RAS International Conference on Humanoid Robots [8] Jason M O’Kane(2014), “A Gentle Introduction to Ros Book” 78 an PHỤ LỤC MÃ NGUỒN CHƢƠNG TRÌNH Chƣơng trình Arduino Due: //Libraries for controlling the encoder #include #include #include #include #include //Librabries for interfacing with ros #include #include #define PWM_l_a // chan phat xung cho encoder #define PWM_l_b #define PWM_r_a 10 #define PWM_r_b 11 Encoder leftEncoder (22, 23); Encoder rightEncoder (24, 25); volatile long oldLeft, newLeft = 0; volatile long oldRight, newRight = 0; volatile long diffLeft = 0, diffRight = 0; double leftVelocity = 0, rightVelocity = 0; double consKp = 25, consKi = 1000, consKd = 0.25; double leftSetPoint = 0, leftPIDOutput; double rightSetPoint = 0, rightPIDOutput; PID PID_leftMotor(&leftVelocity, &leftPIDOutput, &leftSetPoint, consKp, consKi, consKd, DIRECT); PID PID_rightMotor(&rightVelocity, &rightPIDOutput, &rightSetPoint, consKp, consKi, consKd, DIRECT); //Variables for ROS communication ros::NodeHandle nh; std_msgs::Float64 leftWheelVelocity; std_msgs::Float64 rightWheelVelocity; ros::Publisher leftWheelSpeed ("leftWheelSpeed", &leftWheelVelocity); ros::Publisher rightWheelSpeed ("rightWheelSpeed", &rightWheelVelocity); 79 an ///////////////////////////////////////////////////////////////////////////////////////////////////// void leftSetpointSubscriber_cb (const std_msgs::Float64& leftSetpointData) { leftSetPoint = leftSetpointData.data; return; } ///////////////////////////////////////////////////////////////////////////////////////////////////// void rightSetpointSubscriber_cb (const std_msgs::Float64& rightSetpointData) { rightSetPoint = rightSetpointData.data; return; } ///////////////////////////////////////////////////////////////////////////////////////////////////// ros::Subscriber leftSetpointSubscriber ("leftSetpointSubscriber",&leftSetpointSubscriber_cb); ros::Subscriber rightSetpointSubscriber ("rightSetpointSubscriber",&rightSetpointSubscriber_cb); ///////////////////////////////////////////////////////////////////////////////////////////////////// void timer_3_interrupt_for_left_motor () { newLeft = leftEncoder.read (); diffLeft = newLeft - oldLeft; leftVelocity = double(diffLeft) / (4 * 12 * 64) * 10; oldLeft = newLeft; PID_leftMotor.Compute (); return; } //////////////////////////////////////////newLeft/////////////////////////////////////////////////////////// void timer_4_interrupt_for_right_motor () { newRight = rightEncoder.read (); diffRight = newRight - oldRight; rightVelocity = double (diffRight) / (4 * 12 * 64) * 10; oldRight = newRight; PID_rightMotor.Compute(); return; } ///////////////////////////////////////////////////////////////////////////////////////////////////// void timer_5_interrupt_for_sending_to_server () { //Prepare the datat for publishing the left and right velocity in radian 80 an leftWheelVelocity.data = leftVelocity * * M_PI; rightWheelVelocity.data = rightVelocity * * M_PI; //Publish the left and right Velocity leftWheelSpeed.publish (&leftWheelVelocity); rightWheelSpeed.publish (&rightWheelVelocity); return; } ///////////////////////////////////////////////////////////////////////////////////////////////////// void setup () { // setup the pwm input reader pinMode(PWM_l_a, OUTPUT); pinMode(PWM_l_b, OUTPUT); pinMode(PWM_r_a, OUTPUT); pinMode(PWM_r_b, OUTPUT); //Setup parameter for timer for left motor Timer3.attachInterrupt (timer_3_interrupt_for_left_motor); Timer3.start (100000); //100ms //Setup parameter for timer Timer4.attachInterrupt (timer_4_interrupt_for_right_motor); Timer4.start (100000); //100ms //Setup timer for sending data to ROS server Timer5.attachInterrupt (timer_5_interrupt_for_sending_to_server); Timer5.start (100000); //100ms //Setup PID parameters PID_leftMotor.SetMode (AUTOMATIC); PID_rightMotor.SetMode (AUTOMATIC); PID_leftMotor.SetTunings (consKp, consKi, consKd); PID_leftMotor.SetOutputLimits (-255, 255); PID_rightMotor.SetTunings (consKp, consKi, consKd); PID_rightMotor.SetOutputLimits (-255, 255); //Setup ros environment nh.initNode (); nh.advertise (leftWheelSpeed); nh.advertise (rightWheelSpeed); nh.subscribe (leftSetpointSubscriber); nh.subscribe (rightSetpointSubscriber); 81 an } ///////////////////////////////////////////////////////////////////////////////////////////////////// void loop () { //Update the PID value //Update the value of the current setpoint of the left and right motor // For left motor if (leftPIDOutput < 0) { digitalWrite(PWM_l_b, LOW); analogWrite(PWM_l_a, abs(leftPIDOutput)); } else { if (leftPIDOutput > 0) { digitalWrite(PWM_l_a, LOW); analogWrite(PWM_l_b, leftPIDOutput); } } //For right motor if (rightPIDOutput < 0) { digitalWrite(PWM_r_b, LOW); analogWrite(PWM_r_a, abs(rightPIDOutput)); } else { if (rightPIDOutput > 0) { digitalWrite(PWM_r_a, LOW); analogWrite(PWM_r_b, rightPIDOutput); } } //Spin ROS for updating system nh.spinOnce (); } Chƣơng trình hệ điều hành ROS Mã nguồn chƣơng trình chạy hệ điều hành ROS đƣơc lƣu trữ CD đính kèm 82 an S an K L 0 ... ngƣời di chuyển lại gần Robot 73 Hình 5 .32 : Biểu đồ thể tốc độ bánh xe ngƣời di chuyển xa Robot 73 Hình 5 .33 : Tốc độ bánh xe Kp_Linear 0.1 74 Hình 5 .34 : Tốc độ bánh xe... phẩm có tính ứng dụng cao vào thực tiễn, nhóm định chọn đề tài ? ?Lập trình Robot bánh di chuyển theo ngƣời sử dụng camera Kinect? ?? nghiên cứu thi cơng đƣợc Robot có chức di chuyển theo ngƣời thay... LƢỢNG CAO ĐỒ ÁN TỐT NGHIỆP LẬP TRÌNH ROBOT BÁNH DI CHUYỂN THEO NGƢỜI SỬ DỤNG CAMERA KINECT SVTH1: VÕ NGỌC THẨM MSSV : 131 41608 SVTH2: NGUYỄN HỮU MINH ANH MSSV : 131 41005 Khóa : 20 13 – 2017 Ngành