THIẾT kế ROBOT LEO TRỤ và điều KHIỂN

THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN MỤC LỤC Trang tựa TRANG Quy t định giao đ tài i Lý lịch cá nhân ii Lời cam đoan .iii Lời c m ơn iv Tóm tắt v M c l c vi Danh sách hình vii Danh sách b ng .viii Chư ng 1: TỔNG QUAN 1.1 Các k t qu nghiên cứu nước 1.2 M c tiêuvà đối tư ng nghiên cứu .8 1.3 Nhi m v đ tài ph m vi nghiên cứu 1.4 Phương pháp nghiên cứu Chư ng 2: C SỞ LÝ THUYẾT 10 2.1 Cơ sở chọn phần m m mô – lập trình – giám sát .10 2.2Phương pháp u ch độ rộng xung PWM 14 2.3 Phương pháp u n PID 18 2.4 Đi u n đồng tốc độ nhi u động theophương pháp ELS 21 2.5 Phương pháp u n bám vị trí 24 Chư ng 3: KẾT QUẢ MÔ PHỎNG 27 3.1 K t qu mô u n đồng tốc độ hai động 27 3.1.1 Các thông số mô 27 3.1.2 K t qu mô với tỉ l v vị trí vận tốc Kx = 0.7 .32 3.1.3 K t qu mô với tỉ l v vị trí vận tốc Kx = 33 3.1.4 Nhận xét k t qu mô 34 3.2 K t qu mô u n bám vị trí .34 3.2.1 Các thông số mô 34 3.2.2 K t qu mô 36 3.2.3Nhận xét k t qu mô .38 vi THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Chư ng 4: THIẾT KẾ KẾT CẤU C KHệ .39 4.1 Phân tích phương án chọn k t cấu khí 39 4.2 Chọn tr mẫu 39 4.3 Chọn vật li u gia công robot 40 4.4 Chọn động truy n động 42 4.4.1 Phân tích lựa chọn k t cấu động 42 4.4.2 Tính toán lựa chọn công suất động 43 4.5 Chọn bánh xe chủ động 46 4.6Chọn tr c ép 48 Chư ng 5: THIẾT KẾ BỘ ĐIỀU KHIỂN 49 5.1 Chọn c m bi n .49 5.1.1 Encorder 49 5.1.2 SRF05 .51 5.2Chọn vi u n 53 5.2.1 dsPIC30F4011 54 5.2.2 PIC16F628A 55 5.3 M ch công suất 57 5.4 Thi t k giao di n giám sát Visual Basic 6.0 58 Chư ng 6: THUẬT TOÁN ĐIỀU KHIỂN 60 6.1Thuật toán u n bám vị trí cho tr c ép 60 6.2 Thuật toán u n đồng tốc ba động 62 6.3 Công thức sử d ng chương trình u n 65 Chư ng 7: KẾT QUẢ THỰC NGHIỆM .67 7.1 Nhận xét trình di chuy n 67 7.2 Nhận xét k t qu giám sát 69 Chư ng 8: KẾT LUẬN 70 8.1 Những k t qu đ t đư c 70 8.2 H n ch đ tài 70 8.3 Hướng phát tri n đ tài 70 TÀI LI U THAM KH O 71 PHỤ LỤC 72 vi THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN DANH SÁCH CÁC HÌNH Hình 1.1: Robot rắn Hình 1.2: Mẫu robot RISE V2 Hình 1.3: Mẫu robot RISE V3 Hình 1.4: Robot Treebot Hình 1.5: Robot UT-PCR Hình 2.1: Cửa sổ tra cứu thư viện Simulink Hình 2.2: Quá trình lập trình, biên dịch nạp cho PIC Hình 2.3: Cửa sổ làm việc Visual Basic Hình 2.4: Đồ thị dạng xung điều chế PWM Hình2.5 :Sơ đồ nguyên tắc điều khiển tải dùng PWM Hình 2.6: Mạch nguyên lý điều khiển tải PWM Hình 2.7: Giản đồ xung khóa điều khiển đầu Hình 2.8: Điều khiển hồi tiếp với điều khiển PID Hình 2.9: Đáp ứng nấc hệ hở có dạng chữ S Hình 2.10: Đáp ứng nấc hệ kín K = Kgh Hình 2.11: Sơ đồ cấu trúc điều khiển Electronic Lineshaft Hình 2.12: ng dụng điều khiển tùy động vị trí hệ thống bám trụ robot Hình 3.1: Sơ đồ cấu trúc tốc độ động Follower theo động Master Hình 3.2: Mô hình động chiều kích từ không đổi Hình 3.3: Mạch vòng điều khiển nối tầng dòng điện, tốc độ vị trí Hình 3.4: Mô hình mô truyền động đồng vị trí hai động DC theo phương pháp ELS Hình 3.5: Vị trí thực động Master Follower Hình 3.6: Tốc độ động Master Follower Hình 3.7: Vị trí thực động Master Follower Hình 3.8: Tốc độ động Master Follower Hình 3.9: Sơ đồ cấu trúc tốc độ động từ thông không đổi Hình 3.10: Sơ đồ cấu trúc mạch vòng dòng điện Hình 3.11: Mô hình mô hệ điều khiển bám vị trí điều khiển PID vii THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Hình 3.12: Tín hiệu điều khiển Hình 3.13: Vị trí phản hồi sử dụng điều khiển PID Hình 3.14: Tốc độ động sử dụng điều khiển PID Hình 3.15: Dòng điện phản hồi sử dụng điều khiển PID Hình 4.1: Trụ mẫu cho robot Hình 4.2: Bản vẽ tổng quát thiết kế robot Hình 4.3: Thân robot Hình 4.4: Cấu tạo động điện DC trục vít bánh xe Hình 4.5: Góc nghiêng mặt trụ so với mặt đất Hình 4.6: Sơ đồ phân tích lực robot leo trụ Hình 4.7: Động DC trục vít bánh xe dùng cho robot Hình 4.8: Bánh xe từ với đĩa nam châm vĩnh cửu, đĩa sắt từ từ thông Hình 4.9: Độ giảm lực từ bánh xe bao phủ băng cao su Hình 4.10: Bánh xe chủ động dùng cho robot Hình 4.11: Trục ép hành trình 150mm Hình 4.12: Thiết kế khí hoàn thiện Hình 5.1: Cấu tạo Encoder Hình 5.2: Giản đồ xung hai đếm Avà B Hình 5.3: Encoder 100 xung Hình 5.4: Cảm biến siêu âm SRF05 Hình 5.5: Chế độ làm việc thứ SRF05 Hình 5.6: Chế độ làm việc thứ SRF05 Hình 5.7: Sơ đồ chân dsPIC30F4011 Hình 5.8: Sơ đồ chân PIC16F628A Hình 5.9: Mạch nạp PICKIT2-PLUS Hình 5.10: Sơ đồ mạch nguyên lý Eagle Hình 5.11: Sơ đồ mạch cấu tạo Eagle Hình 5.12: Sơ đồ mạch điều khiển hoàn thiện Hình 5.13: Giao diện giám sát Visual Basic 6.0 Hình 7.1: Quá trình di chuỔển robot vii THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN DANH SÁCH CÁC BẢNG Bảng 2.1: Các thông số PID theo phương pháp Zeigler Nichols thứ Bảng 2.2: Các thông số PID theo phương pháp Zeigler Nichols thứ hai Bảng 4.1: Thông số kích thước trụ đứn chiếu sáng Việt Nam Bảng 5.1: Thông số dsPIC30F4011 Bảng 5.2: Thông số PIC16F628A viii THI T K ROBOT LEO TR VÀ ĐI U KHI N Chư ng TỔNG QUAN 1.1 CÁC KẾT QUẢ NGHIÊN C U TRONG VÀ NGOÀI N C 1.1.1 Các m u thi t k robot leo tr 1.1.1.1 Robot rắn Các chuyên gia ch tạo máy Đại học Carnegie Mellon (Mỹ) cho bi t robot r n vừa đ c ch tạo có th bám ch c vào v t liệu, di chuy n vào khu vực nh hẹp mà loại robot khác không th ti p c n Đặc biệt, phần đầu robot đ c trang bị đèn máy quay phim độ nét cao, giúp ng i u n dễ dàng quan sát khu vực tr ớc mặt robot [12] Hình 1.1: Robot rắn Đặc biệt, thi t k c a robot cho phép bám ch c vào loại v t th th i gian r t ng n.Ngay ti p xúc, chi ti t máy c a robot r n s co c m, giúp bám ch c vào loại v t th , b t k kích th ớc ch t liệu Các khớp n i c u thành nên robot ăn khớp với tới m c hoàn h o, cho phép robot r n di chuy n mà không gặp ph i b t k tr ngại Tuy bánh xe nh ng cách di chuy n c a robot r n đ c đánh giá r t cao nh tính linh hoạt c a Các nhà thi t k hy vọng, mẫu robot s nhanh chóng khẳng định đ c kh đặc biệt, phát huy tác d ng việc tìm ki m, c u nạn vùng bị nh h ng th m họa, thiên tai hay th m chí ph c v công tác gián điệp Trang THI T K ROBOT LEO TR VÀ ĐI U KHI N Tuy nhiên, robot r n s ph i tr i qua r t nhi u thử nghiệm khác nhằm hoàn thiện tr ớc đ a vào sử d ng 1.1.1.2 Robot RiSE V2 RiSE robot leo phát tri n b i cty Boston Dynamics (Mỹ),RiSE có hai phiên b n hoàn thành khác ng với b mặt địa hình mà robot sử d ng thi t k ngón chân khác nhau.Phiên b n RiSE V2 robot leo t ng, c i hàng rào [12] Khi robot leo lên cây, RiSE V2 sử d ng kim cong xâm nh p vào v Robot có th thay đổi t th c a đ phù h p với độ cong c a b mặt địa hình Hình 1.2: Mẫu robot RISE V2 RiSE V2 có sáu chân, chân trang bị cặp động điện, chân có haikhớp tự m c t i thi u, có ba chân ti p xúc b mặt c a gỗ.Đi u đ m b o robot bám ch c ch n vào gỗ.Robot có máy tính đ c tích h p thân robot đ u n chuy n động chân,qu n lý thông tin liên lạc c m bi n.Robot đ c u n từ xa b i máy tính xách tay k t n ivới robot thông qua mạng không dây Một ng động c a robot Trang i s u n chuy n THI T K ROBOT LEO TR VÀ ĐI U KHI N 1.1.1.3 Robot RiSE V3 RiSE V3 robot có chân đ c thi t k chuyên d ng r t động, t c độ leo tr cao, ví d nh tr điện thoại tr i Robot có b n chân mạnh m , chân có hai khớp truy n động.B ớc đ c u chỉnh cách linh hoạt [12] Hình 1.3: Mẫu robot RISE V3 Đ leo lên tr điện thoại gỗ, robot sử d ng móng vu t s c nhọn xâm nh p vào gỗ Móng vu t sử d ng cho robot đ c thi t k từ kim phẫu thu t, đ s c bén đ bám ch c vào gỗ Hai chân tr ớc bao xung quanh cột, góc nghiêng phù h p với trọng lực.Hai chân sau bám chặt vào gỗ đ tạo lực đẩy lên trên.Thi t k móng vu t đ m b o cho robot RiSE V3 bám r t ch c 1.1.1.4 Robot Treebot Treebot đ c phát tri n b i nhóm nghiên c u Đại học Trung Qu c (Hồng Kông).Robot đ c thi t k gi ng nh loài sâu, sử d ng t p h p thi t bị truy n động n tính linh hoạt k t n i với hai móng vu t kẹp [12] Trang THI T K ROBOT LEO TR VÀ ĐI U KHI N Hình 1.4: Robot Treebot Trong robot bám chặt vào thân móng vu t th nh t móng vu t th hai s di chuy n đ n vị trí thân bám chặt vào.Sau đó, móng vu t th nh t s m di chuy n v phía tr ớc đ n vị trí mới.Quá trình di chuy n c lặp lặp lại nh Đáng ý móng vu t có b n ngón tay, có th bám chặt vào mạnh m Treebot sử d ng c m bi n đ xác định hình dạng c a cây, từ phân tích b mặt u h ớng đ di chuy n lên thân Treebot s hữu kh tự động, gi m ph c tạp không cần ng i u n.Ng việc i sử d ng đơn gi n yêu cầu robot di chuy n h ớng chung, hệ th ng u n c a robot tự động xác định vị trí đ di chuy n Đ làm u này, robot sử d ng c m bi n c m nh n hình dạng c a Sử d ng hình th c hồi ti p thay cho hồi ti p thị giác hai u m: - Th nh t, cho phép robot ti t kiệm điện, n u robot chạy pin camera s nhanh chóng tiêu th h t điện Trang THI T K ROBOT LEO TR VÀ ĐI U KHI N - Th hai, xử lý thông tin hồi ti p đ c ớc tính tr ớc, kh xử lý thông tin nhanh 1.1.1.5 Robot UT-PCR Các chuyên gia c a Đại học Tehran (Iran) đư phát tri n robot cực leo tr sử d ng bánh xe có tên UT-PCR Nhiệm v c a UT-PCR vệ sinh b i bẩn bóng đèn chi u sáng đ ng cao t c [14] Hình 1.5: Robot UT-PCR UT-PCR có thi t k hình tam giác với sáu bánh xe, ba bánh xe truy n động nằm phía d ới đ c k t n i với động DC, ba bánh xe hổ tr nằm phía nhằm tăng cân cho robot.Theo thi t k c a nó, UTPCR có s tính thu n l i so với robot cực leo tr có: - Trọng l ng robot gi m có chi ti t truy n động - Kh di chuy n nhanh - Robot có th di chuy n nhi u b mặt khác nh tr s t, tr gỗ, tr bê tông, … 1.1.2 Phư ng án thi t k robot leo tr Tr ớc đ a h ớng thi t k ,ta đư đ a nhi u ý t hiện.Sau lựa chọn đ ng có th thực c thi t k tổng th , ta b t đầu thi t k ph n chi ti t khác đ hoàn thành thi t k Các thi t k chi ti tbao gồm: - Thi t k thân robot Trang THI T K ROBOT LEO TR VÀ ĐI U KHI N Chư ng KẾT QUẢ THỰC NGHIỆM 7.1 NHẬN XÉT QUÁ TRỊNH DI CHUYỂN Thao tác thực u n robot: - B ớc 1: C p nguồn cho mạch u n k t n i cáp n i ti p với PC - B ớc 2: n giữ nút FAR đ tr c ép đẩy cho đ n ba bánh xe không ti p xúc với chu vi đáy tr dừng lại, l p robot vào đáy tr - B ớc 3: n nút START đ chuy n tr c ép sang ch độ tự động u chỉnh lực ép vào tr - B ớc 4: Đi u chỉnh bi n tr đ cài đặt t c độ động cho phù h p, thông s t c độ s hi n thị LCD - B ớc 5: n giữ nút UP đ robot di chuy n lên n giữ nút DOWN đ robot di chuy n xu ng - B ớc 6: Khi robot đư xu ng đáy tr , n nút STOP đ chuy n tr c ép sang ch độ u n tay - B ớc 7: n giữ nút FAR đ tr c ép đẩy cho đ n ba bánh xe không ti p xúc với chu vi đáy tr dừng lại, tháo robot kh i tr Hình 7.1: Quá trình di chuỔển robot Trang 67 THI T K ROBOT LEO TR VÀ ĐI U KHI N Trong su t trình robot di chuy n lên xu ng, tr c ép tự động u chỉnh chạy vào đ đ m b o lực ép vào tr không đổi.Lực ép đ từ lực nén lò xo, lò xo đ c nén c tạo chi u dài L Robot di chuy n s làm chi u dài lò xo thay đổi L ± ΔL, c m bi n SRF05 s u chỉnh đ triệt tiêu kho ng cách ΔL, giữ lực ép không đổi Sai s kho ng cách c a SRF05 tr ng h p dao động t i đa 1mm Th i gian u n tr c ép nhanh giúp robot có th di chuy n với t c độ cao Thu t toán u n đồng t c độ ba động đư giúp robot giữ cân t t trình di chuy n, tránh t ng bánh xe bị tr t Khi x y sai s t c độ, u n PID s triệt tiêu sai s nhằm đ a ba động v t c độ Th i gian đ ba động đồng t c nhanh hay ch m ph thuộc vào sai s lớn hay nh Thông s kho ng cách t c độ ba động hi n thị hình LCD giao diện giám sát Visual Basic 6.0, trình truy n nh n liệu nhanh, không x y lỗi Khi robot di chuy n lên, trọng l ng robot lớn nên ta cần cài đặt t c độ cao đ động đ momen kéo robot lên Khi robot di chuy n xu ng, ta nên cài đặt t c độ động th p đ tránh t ng bánh xe bị tr t Đ ki m tra đáp ng c a u n đồng t c ta thực nh sau: - Cho ba động chạy không t i với t c độ c định, ta cho động Master mang t i, t c độ động Master gi m xu ng Ta th y t c độ c a hai động Follower gi m theo t c - Ng c lại, ta cho động Follower mang t i, t c độ động Follower gi m xu ng nh ng nhanh chóng tăng momen đ vọt lên với t c độ động Master Trang 68 THI T K ROBOT LEO TR VÀ ĐI U KHI N 7.2 NHẬN XÉT KẾT QUẢ GIÁM SÁT Thao tác u n giao diện: B ớc 1: Kh i động giao diện Visual basic PC B ớc 2: K t n i cáp truy n liệu từ PC đ n CHIP xử lý mạch u n, đồng th i b t nguồn cho mạch u n B ớc 3: Trên giao diện ta chọn cổng COM cần k t n i b m K T N I B ớc 4: Đ ng t k t n i PC với CHIP xử lý, ta b m NG T Khi b t đầu b m K T N I, t t c liệu từ Chip xử lý s gửi liên t c lên giao diện với t c độ 50ms/lần.Hi n thị s t c độ c a ba động kho ng cách SRF05, đồng th i kèm theo đồ thị t c độ đồ thị bám kho ng cách Trang 69 THI T K ROBOT LEO TR VÀ ĐI U KHI N Chư ng KẾT LUẬN 8.1 NH NG KẾT QUẢ Đ T Đ C - Hi u thêm v k t qu nghiên c u robot leo tr th giới nh ng d ng c a robot thực tiễn - Đ a h ớng thi t k k t c u khí robot leo tr - Tìm ph ơng pháp u n phù h p với mô hình - K t qu mô ph ng góp phần lựa chọn thông s u n phù h p - Bổ sung thêm ki n th c v l p trình C cho CHIP xử lý PIC với phần m m hổ tr - K t h p u n ậ giám sát với mô hình, ghi nh n k t qu thực nghiệm Từ đó, kh c ph c khuy t m đ a h ớng phát tri n 8.2 H N CHẾ C A ĐỀ TÀI - Phần thi t k khí ch a đ học ch a cao, trọng l c đẹp, tồn sai s thi t k , độ b n ng lớn - Động truy n động sử d ng không mang t i đ c th i gian dài - Mô ph ng ch a sát với thực t - Ch a l p ráp thi t bị sơn vệ sinh lên robot 8.3 H NG PHÁT TRIỂN C A ĐỀ TÀI - Thi t k lại phần c ng robot, gi m trọng l ng - Chọn động có momen lớn, ch độ hoạt động dài hạn - ng d ng thêm ph ơng pháp u n nh Fuzzy,… đ tăng tính ổn định cho động Trang 70 Fuzzy logic, PID ậ THI T K ROBOT LEO TR VÀ ĐI U KHI N TÀI LIỆU THAM KHẢO [1] Điều chỉnh truyền động điện tự độngậ Bùi Qu c Khánh - Nhà xu t b n Khoa học & Kỹ thu t ậ Hà nội ậ 1996 [2] Nghiên cứu nâng cao chất lượng hệ thống điều khiển truyền động nhiều động cơ- Ngô Xuân H ng - Lu n văn thạc sỹ - Đại học Hàng h i Việt Nam ậ 2008 [3] Nghiên cứu tổng hợp điều chỉnh mờ lai sử dụng hệ tùỔ động vị trí ậ Lê Qu c Hùng ậ Lu n văn Thạc sĩ ậ Đại học Đà Nẵng [4].Lý thuyết điều khiển tự động thông thường đại - Quyển 1: Hệ tuyến tính- PGS TS Nguyễn Th ơng Ngô ậ Nhà xu t b n Khoa học & Kỹ thu t ậ Hà nội ậ 2004 [5].Lý thuyết điều khiển tuyến tính–GS TS Nguyễn Doưn Ph ớc- Nhà xu t b n Khoa học & Kỹ thu t ậ Hà nội ậ 2009 [6] Lý thuyết điều khiển tự động ậ T.S Nguyễn Thị Ph ơng Hà ậ Th.S Huỳnh Thái Hoàng ậ Nhà xu t b n ĐH qu c gia Tp Hồ Chí Minh - 2005 [7] MATLAP & SIMULINK dành cho kỹ sư điều khiển tự động ậ Nguyễn Phùng Quang [8] Hướng dẫn sử dụng chương trình lập trinh C cho vi điều khiển PIC ậ Trần Xuân Tr ng [9] Visual Basic ậ Created by mercury [10] SIEI – DGFC – ELS Electric line shaft instruction manual [11] LS – Cable – Technical Transfer Document [12] Design and Construction of a tree climbing Robot - Justin Gostanian, Erick Read, Michael A Gennert [13] Realization of a new compact magnetic wheeled climbing robot ậ Ursin Hutter [14] Kinematics Modeling of a Wheel-Based Pole Climbing Robot (UT-PCR)Ali Baghani, Majid Nili Ahmadabadi, Ahad Harati [15] Datashit dsPIC30F4011, PIC16F628A, SRF05 Trang 71 THI T K ROBOT LEO TR VÀ ĐI U KHI N PHỤ LỤC A)CH NG TRỊNH ĐIỀU KHIỂN C A DSPIC30F4011 #include #include #include #include #FUSES NOWDT, XT, NOPUT, NOPROTECT, NODEBUG, NOBROWNOUT //#use rs232(baud=4800,parity=n,bits=8,stop=1,uart2) //#use rs232(baud=4800,parity=n,bits=8,stop=1,uart1) #use rs232(baud=4800, xmit=PIN_f3, rcv=PIN_f2, parity=N, BITS=8, ERRORS, STREAM=pc) #use rs232(baud=4800, xmit=PIN_f5, rcv=PIN_f4, parity=N, BITS=8, ERRORS, STREAM=srf05) #use delay(clock=4000000) unsigned int tocdo=0,chieudai=0; unsigned int docadc=0; int nhan=0,snhan=0,nhanpc=0; int tam1=0,tam2=0,tam3=0; int dsrf05[4]={}; int i=0; int xs1=0,xs2=0,xs3=0,tang=0; int tr=0,ch=0,gui=0; int u1,u_11,u2,u_12,u3,u_13,e1,e_11,e_21,e2,e_12,e_22,e3,e_13,e_23; float A1,A2,A3,T,Kp,Ki,Kd; //****************khai bao cac chan I/O******************* #define nghich input(pin_d2) #define thuan input(pin_d3) #define dao1 pin_c13 #define dao2 pin_c14 #define dao3 pin_b5 void ADC_INIT()//khai bao adc { setup_adc(ADC_CLOCK_INTERNAL | ADC_TAD_MUL_31); setup_adc_ports(sAN4,VSS_VDD); //khai bao chan an0 la chan inout adc docadc= read_adc(); //doc gia tri adc set_adc_channel(4); } /*****************************************************/ #INT_EXT0 //vector ngat ngoai void ngat_ngoai0() { tam1++; } /*****************************************************/ Trang 72 THI T K ROBOT LEO TR VÀ ĐI U KHI N #INT_EXT1 //vector ngat ngoai void ngat_ngoai1() { tam3++; } /****************************************************/ #INT_EXT2 //vector ngat ngoai void ngat_ngoai2() { tam2++; } /***************************************************/ #INT_RDA // vector ngat noi tiep void RDA_isr() { nhanpc=fgetc(pc); if(nhanpc=='r') gui=1; if(nhanpc=='s') gui=0; } #INT_RDA2 // vector ngat noi tiep 628 void RDA2_isr() { nhan=fgetc(srf05); //fputc(nhan,pc); if(nhan=='*') snhan=1; if(nhan=='#') snhan=0; if(snhan==1){dsrf05[i]=nhan;i++;} if(snhan==0){i=0;chieudai=dsrf05[1];} } /******************************************************/ #INT_TIMER3 //vector ngat timer void interrupts_timer3() { SET_TIMER3(55535); tang++; docadc=read_adc(); tocdo=docadc/16; if(tang>=60) { tang=0; xs1=tam1; xs2=tam2; xs3=tam3; tam1=0; tam2=0; tam3=0; Trang 73 THI T K ROBOT LEO TR VÀ ĐI U KHI N } } //khai bao chuong trinh void ADC_INIT(); void RDA_isr(); void RDA2_isr(); void ngat_ngoai0(); void ngat_ngoai1(); void ngat_ngoai2(); void interrupts_timer2(); void dongtoc1(); void dongtoc2(); void PID_init(); void dongtoc3(); //khai bao chuong trinh chinh void main() { ADC_INIT(); //khoi dong adc lcd_init(); //khoi dong lcd PID_init();//khoi dong PID SETUP_TIMER3(TMR_INTERNAL|TMR_DIV_BY_1); //khoi dong timer SET_TIMER3(55535); //dat gia tri timer ENABLE_INTERRUPTS(INT_TIMER3); //khoi dong ngat timer enable_interrupts(int_rda); //khoi dong ngat noi tiep1 enable_interrupts(int_rda2); //khoi dong ngat noi tiep2 ENABLE_INTERRUPTS(INT_EXT2); //khoi dong ngat ngoai ENABLE_INTERRUPTS(INT_EXT1); ENABLE_INTERRUPTS(INT_EXT0); EXT_INT_EDGE(L_TO_H); //khoi dong ngat ngoai xung tu thap len cao ENABLE_INTERRUPTS(INT_EXT2); //khoi dong ngat ngoai enable_interrupts(INTR_GLOBAL); //khoi dong ngat toan cuc //lcd_gotoxy(1,1); //printf(lcd_putc,"LUAN VAN TH.S"); //lcd_gotoxy(1,2); //printf(lcd_putc,"ROBO LEO TRU"); setup_motor_pwm(1,MPWM_FREE_RUN,1,0,2000); set_motor_unit(1,1,MPWM_ENABLE_H|MPWM_FAULT_NO_CHANGE,0,0) ; set_motor_unit(1,2,MPWM_ENABLE_H|MPWM_FAULT_NO_CHANGE,0,0) ; set_motor_unit(1,3,MPWM_ENABLE_H|MPWM_FAULT_NO_CHANGE,0,0) ; set_motor_pwm_duty(1,1,0); set_motor_pwm_duty(1,2,0); set_motor_pwm_duty(1,3,0); while(1) Trang 74 THI T K ROBOT LEO TR VÀ ĐI U KHI N { lcd_gotoxy(1,1); printf(lcd_putc,"V/P:%3d",xs1); lcd_gotoxy(9,1); printf(lcd_putc,"%3d",xs2); lcd_gotoxy(14,1); printf(lcd_putc,"%3d",xs3); lcd_gotoxy(1,2); printf(lcd_putc,"T_on %4d",tocdo); lcd_gotoxy(11,2); printf(lcd_putc,"K/c%3d",chieudai); if(gui==1) { fputc(xs1,pc); fputc(xs2,pc); fputc(xs3,pc); fputc(chieudai,pc); } if(thuan==0) { output_low(dao1); output_low(dao2); output_low(dao3); delay_ms(15); set_motor_pwm_duty(1,1,tocdo); // set_motor_pwm_duty(1,2,tocdo); // set_motor_pwm_duty(1,3,tocdo); dongtoc3(); } if(nghich==0) { output_high(dao1); output_high(dao2); output_high(dao3); delay_ms(15); set_motor_pwm_duty(1,1,tocdo); // set_motor_pwm_duty(1,2,tocdo); // set_motor_pwm_duty(1,3,tocdo); dongtoc3(); } if((thuan==1)&&(nghich==1)) Trang 75 THI T K ROBOT LEO TR VÀ ĐI U KHI N { set_motor_pwm_duty(1,1,0); set_motor_pwm_duty(1,2,0); set_motor_pwm_duty(1,3,0); // u2=0; PID_init(); } } } void dongtoc3() { set_motor_pwm_duty(1,1,tocdo); set_motor_pwm_duty(1,2,u2); set_motor_pwm_duty(1,3,u3); e2= xs1-xs2 ; //chieudai-dem; // tinh sai so xac lap u2=u_12 + A1*e2 + A2*e_12 + A3*e_22; // tin hieu dieu khien //u(k)=u(k-1)+A1*e(k)+A2*e(k-1)+A3*e(k-2) u_12=u2; // cap nhat u(k-1), e(k-1), e(k-2) e_22=e_12; e_12=e2; if(u2>=4000) {u2=4000;u_12=4000;} if(u2=4000) {u3=4000;u_13=4000;} if(u389){near();} if((distance=87)) {stop();} if(distance[...]... chân Robot nhi u chân là một mẫu robot phân đoạn với hai chân cho mỗi phân đoạn Thi t k dựa trên hoạt động c a một con r t Thi t k này s cho phép chúng ta dễ dàng đi u h ớng cho robot trong lúc nó di chuy n [12] Thi t k này s không phù h p với đ tài vì s l ng c a bộ ph n chuy n động và kích th ớc c a robot s khá lớn, và nó s không th hoàn Trang 6 THI T K ROBOT LEO TR VÀ ĐI U KHI N thànhvới nguồn lực và. .. ớc robot ph i ph thuộc vào chu vi tr 1.2 MỤC TIểU VÀ Đ I T NG NGHIÊN C U Trang 7 THI T K ROBOT LEO TR VÀ ĐI U KHI N M c tiêu c a lu n văn là mô ph ng, ki m ch ng, thi t k và ng d ng bộ đi u khi n cho Robot leo tr Bộ đi u khi n này ph i đ m b o các tiêu chuẩn: - T c độ c a ba động cơ ph i luôn đồng bộ với nhau - Khi đ ng kính tr thay đổi thì bộ ph n tạo lực ép tùy chỉnh sao cho lực ép bánh xe vào... u xung PWM - Th hai, đi u khi n động cơ tạo lực ép theo ph ơng pháp đi u khi n bám vị trí 1.3 NHIỆM VỤ C A ĐỀ TÀI VÀ PH M VI NGHIÊN C U 1.3.1 Nhi m v c a đ tài Đ thực hiện m c tiêu trên, lu n văn Thiết kế Robot leo tr và điều khiển t p trung các v n đ sau: - Thi t k k t c u cơ khí cho Robot - Lựa chọn ph ơng pháp đi u khi n phù h p - Mô ph ng đ ki m tra độ tin c y c a bộ đi u khi n - Thi t k cơ c... đi u khi n và remote đi u khi n - L p trình vi đi u khi n theo thu t toán c a bộ đi u khi n đư thi t k - Ki m tra k t qu đạt đ c và đi u chỉnh sai s thông qua hoạt động thực t c a Robot - Nh n xét k t qu và k t lu n 1.3.2 Ph m vi nghiên c u Trang 8 THI T K ROBOT LEO TR VÀ ĐI U KHI N - Robot đ 200mm và đ c thi t k phù h p với những tr có đ ng kính đỉnh không nh hơn 50mm - Bộ ph n phun sơn và vệ sinh... DC 2.3 PH NG PHÁP ĐIỀU KHIỂN PID 2.3.1 Gi i thi u b đi u khi n PID Bộ đi u khi n PID đ c sử d ng r t rộng rưi trong thực t đ đi u khi n các hệ SISO theo nguyên lý hồi ti p Cácđ i t ng đi u khi n phổ bi n nh nhiệt độ lò nhiệt, t c độ động cơ, mực ch t l ng trong bồn ch a w + e - PID u Đối tượng ĐK y Hình 2.8: Điều khiển hồi tiếp với bộ điều khiển PID Trang 18 THI T K ROBOT LEO TR VÀ ĐI U KHI N N u... độ quay và dòng điện: máy làm gi y, máy dệt nhi u tr c sử d ng nhi u động cơ có th coi là hệ th ng tùy động đồng t c…[3] 4 6 7 5 ΔL L 2 L’ L Lực F’ Lực F L ΔL 1 3 Ghi chú: 1 Động cơ truỔền động bánh xe; 2 Bánh xe; 3 Tiết diện trụ; 4 Động cơ nén lò xo; 5 Lò xo nén; 6 Trục vít nén lò xo; 7 Bánh xe truyền động với trục vít Trang 25 Lực F THI T K ROBOT LEO TR VÀ ĐI U KHI N Hình 2.12: ng dụng điều khiển tùỔ...THI T K ROBOT LEO TR VÀ ĐI U KHI N - Lựa chọn cách th c di chuy n - Thi t k chân robot - Lựa chọn động cơ - Chọn thi t bị đi u khi n - Chọn phần m m hổ tr 1.1.2.1 Thi t k b n chân Thi t k cơ b n nh t mà chúng ta đư th y trong nghiên c u là một thi t k b n chân nh robot RiSE V3.Bằng cách sử d ng b n chân trên robot, ta gi m s chi ti t truy n động và công su t tiêu th khi robot di chuy n.Thi... lý điều khiển tải bằng PWM Trang 16 THI T K ROBOT LEO TR VÀ ĐI U KHI N Ugs t on t off 0 t T Ud 0 t Hình 2.7: Giản đồ xung của khóa điều khiển và đầu ra Nguyên lý: Trong kho ng th i gian ton, ta cho van G m , toàn bộ điện áp nguồn Ud đ c đ a ra t i Còn trong kho ng th i gian toff, cho van G khóa, c t nguồn cung c p cho t i.Đi u quan trọng nh t cần chú ý c a một tín hiệu PWM chính là chu kỳ và tần s... 4.104, bao gồm : PCB, PCM và PCH , l p trình cho các họ PIC 10 bit,12 bit, 14 bit, PIC 18 và DsPIC Trình biên dịch c a PIC C compiler s chuy n ch ơng trình theo chuẩn c a C thành dạng ch ơng trình theo mư Hexa (file.hex) đ nạp vào bộ nhớ c a PIC Quá trình chuy n đổi đ c minh hoạ nh Hình 2.2 Trang 12 THI T K ROBOT LEO TR VÀ ĐI U KHI N Hình 2.2: Quá trình lập trình, biên dịch và nạp cho PIC 2.1.3 Gi i... đặc đi m c a đ i t ng Cách 1:Dựa vào đáp ng quá độ c a hệ h , cách này áp d ng cho các đ it ng có đáp ng đ i với tín hiệu vào là hàm n c có dạng chữ S, ví d nh nhiệt độ lò nhiệt, t c độ động cơ,…[6] Hình 2.9: Đáp ứng nấc của hệ hở có dạng chữ S Trang 19 THI T K ROBOT LEO TR VÀ ĐI U KHI N Bảng 2.1: Các thông số PID theo phương phápZeigler và Nichols thứ nhất Cách 2:Dựa vào đáp cácđ i t ng quá độ c a hệ