Nghiên cứu cấu trúc và phát triển chương trình điều khiển thông minh của các robot nhiều bậc tự do

ĩn ? n ì [%ỜC V ĐẠI HỌC QUỐC GIA HÀ NỘI KH O A CÔNG NGHỆ T rầ n T h ị T h ú y H NGHIÊN CỨU CẤU TRÚC VÀ PHÁT TRIẺN CHƯƠNG TRÌNH ĐIỂU KHIEN t h ô n g m i n h c ủ a CÁC ROBOT NHIỂU BẬC TỤ DO Chuyên ngành : KỸ THUẬT v TUYẾN ĐIỆN TỬ VÀ THƠNG TIN LIÊN LẠC Mã SỐ : 2.07.00 ĐAI HOC O O Ố C ' V>A HÁ NỌ! Ị TRUNG TÁM Tỉí MG T-.N Tì i lĩ V?ỆN ỉ J _ i LU ẬN VĂN TH ẠC sĩ KH O A HỌC NGUỜI HUỐNG D Ẫ N KHOA HỌC T S T r ầ n Q u a n g V in h T S L ẻ V ă n N g ự Mục lục Lời cảm ơn Lời nói đầu Phần lý thuyết Chương Các vấn đề công nghệ Robot 1.1 Các đặc điểm chung 1.2 Các phận Robot 1.2.1 Bộ thao tác 1.2.2 Bộ chấp hành 1.2.3 Nguồn nuôi 1.2.4 Bộ điều khiển 1.3 Bộ xương Robot 1.4 Các hệ Robot Chương Hệ thống điều khiển 2.1 Nguyên tắc hoạt động Robot 2.1.1 Hoạt động Robot nonservo 2.1.2 Hoạt động Servo Robot 2.2 Các yếu tố động Robot 2.2.1 Độ ổn định 2.2.2 Độ phân giải điều khiển 2.2.3 Độ phân giải khơng gian 2.2.4 Độ xác tuyệt đối 2.2.5 Độ xác lặp lại 2.2.6 Độ mềm dẻo Chương Sensor 3.1 Các sensor Robot 3.2 Phân loại sensor 3.2.1 Chức trình diễn 3.2.2 Định vị phát 3.2.3 Sự phóng xạ vật lý 3.3 Vi chuyển mạch 3.4 Sensor cự ly gần (Proximity) 3.4.1 Sensor cảm ứng 3.4.2 Sensor hiệu ứng Hall 3.4.3 Sensor điện dung 3.4.4 Sensor siêu âm 3.4.5 Sensor quang cự ly gần (proximity) 3.5 Sensor xúc giác 3.5.1 Sensor nhị phân 3.5.2 Sensor tương tự 3.6 Sensor quang điện Chương Động học tay máy Robot 4.1 Cấu trúc cấu tay máy 4.2 Vị trí điểm tay máy 4.2.1 Biến đổi toạ độ chuyển động quay 4.2.2 Biến đổi toạ độ chuyển động tịnh tiến Trang 2 2 4 12 13 15 15 17 18 20 20 22 23 24 25 27 29 29 29 30 30 33 34 35 35 37 38 40 41 41 42 42 43 46 46 46 47 48 4.2.3 Biểu diễn toạ độ hệ toạ độ đồng 4.3 Phép quay tổng quát 4.4 Phương trình động học Robot 4.4.1 Phép quay EULẼR 4.4.2 Phép quay Roll, Pitch, Yaw 4.3 Xác đính vị trí 4.5 Thiết kế quỹ đạo Robot 4.5.1 Các khái niệm toán quỹ đạo 4.5.2 Quỹ đạo đa thức bậc 4.5.3 Quỹ đạo tuyến tính với cung hai đầu Parabol LSPB 4.5.4 Quy đạo BANG BANG PARABOLIC BLEND (BBPB) Phần Thực Nghiệm 5.1 Hexapod II PE Robot Kit 5.1.1 Chip vi điều khiển BASIC Stamp 2e 5.1.2 Mạch điều khiển servo nối tiếp s s c 5.1.3 Detector hồng ngoại cự ly gần (IRPD infrared Proximity Detector) 5.1.4 Mạch hiển thị tinh thể lỏng ILM-216 5.1.5 Giới thiệu ngôn ngữ BASIC STAMP 5.2 Điều khiển tay máy ED-7220 5.2.1 Tay máy ED-7220 5.2.2 Bộ điều khiển ED-MK4 5.2.3 Xác định khơng gian làm việc tốn ngược củaED-7220C Kết luận Phụ [ục 49 50 56 57 58 59 62 63 64 65 68 70 70 71 75 79 82 85 97 97 99 100 106 Lời m đầu Trong thập kỷ qua công nghệ điện tử cồng nghệ phần mềm phát triển cách vũ bão, kéo theo cơng nghệ khác đạt thành tựu to lớn Một ngành cơng nghiệp có ảnh hưởng lớn Robot Mặc dù thuật ngữ Robot đời từ năm 20 kỷ 20 thời gian dài phát triển Robot chủ yếu phụ thuộc vào phát triển ngành khí Các Robot thường làm việc lặp lặp lại cứng nhắc nên ứng dựng hạn chế số ngành cụ thể luyện kim, khai thác mỏ, hàn Cùng với phát triển xã hội, nhu cầu người ngày gia tăng Yêu cầu phải tạo Robot thông minh hơn, linh hoạt hoạt động “suy nghĩ’ gần với người hơn, tự làm chủ có nghĩa Robot tự đưa chiến lược hoạt động số tình cụ thể Để giải u cầu ngành cơng nghiệp Robot phải kết hợp nhiều ngành, nhiều lĩnh vực khác : Cơ khí, điện tử, tin học đặc biệt công nghệ xử lý tín hiệu xử lý ảnh, xử lý âm Công nghệ robot giới đạt thành tựu lớn, Việt Nam lĩnh vực nhiều mẻ chưa đạt nhiều thành tựu Với mong muốn cho cán sinh viên khoa Công nghệ, đặc biệt sinh viên môn Điện tử Kỹ thuật máy tính làm quen, tìm hiểu với cơng nghệ robot Đồng thời, với cố gắng thầy mơn mong muốn xây dựng phịng thí nghiệm robot đại Khoa Hiện nay, môn nhập phần cứng số robot hãng tiếng giới sản xuất Công việc nghiên cứu phát triển phần mềm cho robot Đây yêu cầu đặt cho luận văn Với luận văn chúng tơi tiến hành tìm hiểu, lắp đặt viết phần mềm điều khiển hai robot Hexapod II PE hãng Lynxmotion ED-7220 Hàn Quốc P H Ầ N LÝ T H U Y Ế T CHƯƠNG CÁC VẤN ĐỂ c BẢN TRONG CƠNG NGHỆ ROBOT Cơng nghệ Robot ứng dụng khoa học, kết hợp cơng cụ khí ứng dụng máy tính Nó bao gồm nhiều lĩnh vực khác thiết kế khí, lý thuyết điều khiển, vi điện tử, lập trình máy tính, trí tuệ nhân tạo, nhân tố người, lý thuyết chế tạo Ngày nay, việc nghiên cứu phát triển luôn tiếp diễn nhằm cải tiến cách làm việc hành vi Robot Sự tiến công nghệ làm tăng phạm vi ứng dụng Robot tương lai 1.1 Các đặc điểm chung Phát triển ngành công nghiệp Robot cách mạng thiết bị tự động hoá Robot dụng cụ chuyên dụng linh hoạt, khác với thiết bị tự động hố cố định Bằng cách thêm sensor, Robot có khả thích nghi với mơi trường làm việc thay đổi ảnh hưởng chúng điều kiện làm việc khác Robot công nghiệp trở thành cơng nhân khí “smarter”(nơng cạn) nay, chấp nhận cách rộng rãi Robot cơng nghiệp cơng cụ khí hay cánh tay khí Thật khơng phù hợp nghĩ người máy bản, Robot cánh tay khí, đặt sàn, máy, trần nhà, tường để thích hợp với cánh tay khí, dạy thực nhiệm vụ lặp lặp lại môi trường định Trong hầu hết trường hợp, Robot thường không dịch chuyển, khơng có khả nhìn nhận biết khơng gian làm việc Mặc dù có hạn chế Robot đóng vai trị quan trọng hoạt động công nghiệp Một khoảng trống lĩnh vực Robot khơng có tính linh hoạt cao người Robot có nhiều lợi ích, đơn giản máy nên khơng bị mệt mỏi, buồn tẻ, làm việc mồi trường ồn, nóng, nguy hiểm Robot hoạt động tốt điều kiện căng thẳng, nguy hiểm, bẩn tuỳ theo tính chất cơng việc Robot dịch chuyển cánh tay theo hướng vùng làm việc Đặc điểm dịch chuyển phụ thuộc nhiều vào cấu tạo 1.2 Các p hận Robot Hầu hết Robot cấu tạo từ số thành phần thao tác (manipulator), chấp hành (là phần thao tác), nguồn nuôi, phận điều khiển Hình 1-1 minh hoạ cách rõ ràng mối quan hệ bốn thành phần Robot cơng nghiệp điển hình Hình l - l Các thành phần Robot công nghiệp Bộ thao tác gọi tay Robot bao gồm khớp cho phép trục chuyển động theo nhiều hướng khác Bộ phận chấp hành (end-effector) dụng cụ kẹp, thiết bị đặc biệt, gắn cố định vào tay Robot để thực công việc giao Nguồn nuôi cung cấp điều chỉnh lượng để biến thành chuyển động nhờ phát dộng Robot, điện, khí, hay nước Bộ điều khiển xác định trạng thái ban đầu, định giới hạn, tọa độ cho chuyển động Và chấp nhận lối vào cần thiết cho Robot cung cấp lối để ghép nối với giới bên ngoài, 1.2.1 Bộ thao tác Bộ thao tác phận khí cung cấp chuyển động tương tự cánh tay người Chức nguyên thuỷ cung cấp chuyển động đặc biệt, cho phép cơng cụ phần cuối cánh tay thực công việc giao Sự chuyển động Robot chia làm hai loại chính: chuyển động cánh tay, chuyển động người (vai khuỷu tay) chuyển động cổ tay Chuyển động khớp riêng lẻ kết hợp với hai loại gọi bậc tự Mỗi trục bậc tự Robot cơng nghiệp điển hình thường có từ đến bậc tự cổ tay chạm điểm không gian với định hướng đặc biệt nhờ ba chuyển động: pitch (sự bồng bềnh), hay chuyển động lên-xuống; yaw (sự lệch hướng), chuyển động từ bên sang bên (side to side); chuyển động roll hay chuyển động quay Các khớp gán nhãn pitch, yaw, roll gọi trục định hướng Do vậy, thao tác phần Robot để thực công việc giao Các điểm nằm khuỷu, chỗ trượt, hay chỗ quay gọi khớp hay trục vị trí Bộ thao tác thực cách sử dụng thiết bị khí liên kết với nhau, bánh răng, phát động, thiết bị phản hồi Các trục vị trí gọi hệ toạ độ cầu Hệ toạ độ cầu xác định khơng gian cố định, đó, thao tác hoạt động hệ toạ độ tuyệt đối 1.2.2 Bộ chấp hành Robot trở thành máy cơng nghiệp gắn lên tay máy công cụ hay thiết bị, thường gọi chấp hành, ngồi hay gọi cơng cụ cuối tay (EOAT: End Of Arm Tooling) Dựa vào yêu cầu cụ thể, tay máy gắn chấp hành sau: > Kẹp (gripper), móc (hook), xúc (scoop), nam châm điện, ống chân khơng, ngón tay dính cầm vật liệu > Súng phun dùng sơn > Hàn cung cắt cung > Dụng cụ công suất mũi khoan, > Các dụng cụ chuyên dụng vật gán cố định máy lắp đặt > Các thiết bị đo thị quay số, đo độ sâu, tương tự Điểm công cụ điểm gốc hệ toạ độ điểm tiếp giáp công cụ tay Robot 1.2.3 Nguồn nuôi Chức nguồn nuôi cung cấp điều chỉnh lượng Robot hoạt động Nguồn nuôi chủ yếu Robot công nghiệp điện Thứ đến khí, cuối thuỷ lực Một số hệ Robot yêu cầu ba loại nguồn Nguồn liên quan trực tiếp đến tỷ lệ tải trọng Robot Đối với loại nguồn, phát động điều khiển có mối quan hệ với đặc điểm, ưu điểm hạn chế chúng 1.2.4 Bộ điều khiển Bộ điều khiển dùng để truyền xử lý thông tin xác định trạng thái ban đầu, định giới hạn, toạ độ chuyển động liên tục Robot Nó chấp nhận lối vào cần thiết với Robot cung cấp lối tín hiệu để điều khiển Robot phát động phải phù hợp với chuyển động Robot giới Trái tim điều khiển máy tính nhớ bán dẫn Lối vào lối hệ thống điểu khiển phải cung cấp giao diện truyền thông máy tính điều khiển Robot phần sau: > Sensor phản hồi > Thiết bị dạy học > Thiết bị lưu trữ chương trình > Thiết bị copy cứng > Các thiết bị phần cứng khác Máy tính điều khiển chuyển động cánh tay Robot cách truyền tín hiệu thơng qua giao diện đến phát động Robot Mỹ, Robot thường chia làm loại chính: > Nonservo > Servo > Servo điều khiển (Servo-controlled) Nonservo hệ thống vòng mở, servo hệ thống vịng đóng Trong hệ thống vịng mở, tín hiệu lối khơng phụ thuộc vào lối hệ thống, hệ thống vịng đóng, lối điều khiển ln so sánh với lối vào thông qua thiết bị phản hồi để hai đại lượng sử dụng thời tạo kết theo mong muốn Robot servo-điều khiển hệ thống vòng đóng có đường điều khiển liên tục Robot nonservo dạng đơn giản mặt cấu trúc hoạt động Đó loại: Robot giới hạn trình tự (limited-sequence), Robot nhấc đặt, Robot bang-bang Các thiết bị điều khiển chuyển đổi phận quan trọng Robot Chúng truyền thông tin đến điều khiển vịtrí biến thiêncủa khớp Robot liên kết chúng Robot có hệ thống vịng đóng hay vòng mở điều khiển chuyển động tay máy thơng qua chương trình điều khiển Điểm khác biệt loại hệ thống vịng mở khơng có sensor cánh tay Robot để cung cấp tín hiệu phản hổi Tín hiệu phản hổi vị trí tay máy cho điều khiển Do vậy, hệ thống vòng mở điều khiển liên tục điều chỉnh tay máy chạm tới điểm dừng cố định Nonservo (vòng mở) Hệ thống Robot nonservo hình 1-2 Sơ đồ sử dụng Robot có bốn trục chạy khí Tại điểm bắt đầu chu trình, điều khiển tạo chuyển động cho Robot thông qua bước biến đổi liên tục Tại bước đầu tiên, điều khiển gửi tín hiệu đến giá trị điều khiển thao tác Nguồn cung cấp khơng khí Hình 1-2 Hệ thống Robot nonservo (vòng mở) Khi van điều khiển mở, khơng khí qua phát động hay xi lanh làm cho xi lanh chuyển động Khi van cịn mở phần thao tác tiếp tục chuyển động bị ngăn lại điểm dừng cuối xilanh Sau xi lanh đến khoảng cách mong muốn cơng tắc giới hạn kích hoạt Ta nói điều khiển đóng van điểu khiển Sau điều khiển gửi tín hiệu đến van điều khiển để đóng nó, chuyển động bước chương trình xác lập lại tín hiệu cần thiết Q trình xử lý tiếp tục tất bước chương trình hồn thành Đối với chuyển động Robot nonservo, phận thao tác chuyển động nghiêng đạt kết mong muốn Cánh tay Robot dừng lại chạm đến mục tiêu theo số phương pháp sau: > Bộ phát động > Điểm dừng cố định > Điểm dừng biến thiên > Mô tơ bước Bộ phát động Robot thiết bị dùng để biến đổi lượng thành chuyển động Các thiết bị xi lanh thuỷ lực, xi lanh khí cuộn dây phát điện mơ tơ, xilanh chuyển động từ vị trí gần đến vị trí xa cánh tay Robot Bộ phát động tuyến tính hình 1-3 1-4 Cả hai phát động khí nước cung cấp lượng cách dịch chuyển chất lỏng Trong trường hợp thứ nhất, chất lỏng nén khí, trường hợp thứ hai chất lỏng thường nén dầu Sự hoạt động phát động thường giống ngoại trừ khả nén dòng Hệ thống khí điển hình hoạt động với áp lực khoảng 100 pound/inch2 hệ thống nước khoảng từ 1000 đến 3000 pound/inch2 Xilanh khơng khí Lơi khơng khl' Thanh pittông c° tác đôn e kéo □ C o lại Kẹp -V77777 Giới hạn co Hình 1-4 Robot nhấc đặt hãng Yamaha có trục tự Có hai mối quan hệ ta đề cập đến phát động: vận tốc pit-tông phát động lực tác động lên lối phát động với công suất lối vào Lực tác động lên lối vào vận tốc pit-tơng tác động kép xi lanh, khơng giống chế giãn co Hiện tượng xảy hiệu ứng pit-tông định nghĩa phương trình sau: Đối với chuyển động giãn, ta có: serp o s(lO ) = 127 '1 l e f t re a r H (- forw ard) s e r p o s ( ll) = 195 '2 l e f t re a r V (- l i f t ) gosub so u t re tu rn fo rw a rd : gosub s 0_ h f gosub s3 gosub so u t l l t d gosub s i gosub s 0_ hb gosub so u t gosub s 0_ hb gosub s i gosub so u t gosub s _ lltu tu td gosub s 0_ hf gosub so u t re tu rn backup: s e ro u t fo r y 13, $418C, [16, = to "16 " , "B acking Up "] "16 " , "T urning L eft "] "16 " , "W alking Forw ard "] "16 "»"T urning R ight "] ste p s gosub s _ 0hb gosub s _ lltd gosub so u t gosub s l_ tu gosub sQ _10hf gosub s o u t gosub s _ 10hf gosub s l_ td gosub s o u t gosub s _ lltu gosub s 0) _ h b gosub so u t n ex t t u r n _ l e f t : s e ro u t , $ C ,[16, m aneuver fo r y = = m aneuver to +1 ste p s gosub s _ 10t r n l gosub s _ lltd gosub so u t gosub s l_ tu gosub s _ 10tr n lb gosub so u t gosub s _ 10tr n lb gosub s l_ td gosub so u t gosub s _ lltu gosub s _ tr n l gosub s o u t n ext s e ro u t , $ C ,[16, re tu rn t u r n _ r ig h t: s e ro u t , $ C ,[16, m aneuver fo r y = gosub = m aneuver to ste p s s _ 10tr n lb gosub s _ lltd gosub s o u t gosub s l_ tu +1 g o su b s 0_ t r n l g o su b sout g o su b s 0_ 10t r n l g o su b s l_ td g o su b sou t g o su b s _ lltu g o su b s 0_ t r n l b g o su b sout next s e r o u t ,$ C ,[16, "16 " , "W alking r e tu r n sub _ b ack u p : gosuib g oto backup s t a r t su b _ tu irn _ le f t : gosuib g oto tu r n _ le f t S itart su b _ tu irn _ rig h t : gosuib g oto tu rn _ r ig h t s ;ta rt u n stic :k : step> s = gosuib tu r n _ le f t m aneuver s te p s g o to = = S itart s o u t : fo r x = to se;ro u t 11 ,$ ,[2 5 ,x ,s e r p o s ( x ) ] next pausie d e la y re tu rm s0_10hif: 'H o riz o n ta l Forw ard serp)os(O ) = s e r p o s (0 ) - s tr id e serp>os(2 ) = s e r p o s (2 ) + s tr id e serp>os(4) = s e r p o s (4) - s tr id e serp io s(6 ) = se rp o s (6 ) - s tr id e serp> os(8 ) = s e r p o s (8 ) + s tr id e serp)os(lO ) = s e r p o s (10) - s tr id e re tu rm s _ llt.d : 'T rip o d Down serpjos(3) = s e r p o s (3) - l i f t serpjos(7) = s e r p o s (7) + l i f t serp > o s(ll) = s e r p o s (1 ) + l i f t re tu rm sl_ tu i : 'T rip o d Up serp>os (1 ) s e r p o s (1 ) + l i f t s e r p > o s (5) s e r p o s (5) + l i f t s e r p > o s (9) s e r p o s (9) - l i f t re tu rm s _ hib : 'H o riz o n ta l Back s e r p i o s (0 ) s e r p o s (0 ) + s e r p ) o s (2 ) s e r p o s (2 ) - s tr id e s tr id e s e rp io s (4 ) s e r p o s (4) + s tr id e serp)os (6 ) s e r p o s (6 ) + s tr id e s e r p io s (8 ) s e r p o s (8 ) - s tr id e Forw ard "] = s e r p o s (1 ) s e r p o s (1 ) + s tr id e r e tu r n s l_ td : 'T rip o d Down s e r p o s (1 ) = s e r p o s (1 ) - l i f t s e r p o s (5) = s e r p o s (5) - l i f t s e r p o s (9) = s e r p o s (9) + l i f t r e tu r n 'T rip o d s3 _ l lt.u : Up s e r p o s ' 3) = s e r p o s (3) + l i f t s e r p o s ' 7) = se rp o s(7 ) - l i f t s e r p o s i1 ) = s e r p o s ( ll) - l i f t r e tu r n 'T rip o d s 0_ l 0t : r n l : ? s e r p o s (0 ) s e r p o s (0 ) - s tr id e s e r p o s (2 ) s e r p o s (2 ) s tr id e s e r p o s (4) s e r p o s (4) - s tr id e s e r p o s (6 ) se rp o s (6 ) -f s tr id e s e r p o s (8 ) s e r p o s (8 ) - s tr id e s e r p o s (1 ) = s e r p o s (1 ) + s tr id e r e tu r n s 0_ l 0t : r n l b : 'T rip o d ? s e r p o s (0 ) s e r p o s (0 ) s tr id e serp > o s (2 ) s e r p o s (2 ) s tr id e s e r p o s (4) s e r p o s (4) s tr id e s e r p o s (6 ) s e r p o s (6 ) s tr id e s e r p o s (8 ) s e r p o s (8 ) s tr id e serp)os(lO ) = s e r p o s (10) - s tr id e r e t u r n BS2E} ' {$STMMP ' H E BSE 'T h is is 'D e fin ie th e RAM M antis / S p id e r used serpos v a r b y t e (1 ) l i f t v a r b y te strid e? v a r b y te d elay v a r w ord m aneuver v a r n ib ste p s v a r n ib tem pi v a r n ib tem pr v a r n ib X v a r n ib y v a r n ib z v a r n ib a v a r n ib b v a r w ord rex v a r w ord rcy v a r w ord s t a r t : gosuib homepos gosuib so u t s e ro m t ,$ C , [ , "M antis gosuib s _ lltu gosuib so u t m a n tis;: gosuib backup 'p r e p a re / S p id e r th re e leg s "] to b eg in w alking c y cle g o su b homepos gosuib sout pause 0 s e r p o s (0 ) = s e r p o s (0 ) + s tr id e + se rp io s (1 ) = s e r p o s (1 ) + l i f t 25 s e r p io s (2 ) = s e r p o s (2 ) + s tr id e se rp io s (4 ) = s e r p o s (4) = s e r p o s (5) + s tr id e s e r p i o s (5) serpios (6 ) = s e r p o s (6 ) - s tr id e - s e r p i o s ( 7) = s e r p o s ( 7) - l i f t 25 s e r p i o s ( 8) = s e r p o s ( 8) - s tr id e + - = s e r p o s (1 ) + s tr id e serpios (1 ) = s e r p o s (1 ) - l i f t pause 254 'm ax 254 65 ' m in 'm in sout 0 gosuib homepos gosuib sou t pause 100 gosuib s _ lltu gosuib sout gosuib forw ard gosuib homepos gosuib sout pau se 100 s p id e r : serp>os(0) = serp os(O ) + s tr id e s e rp io s d ) = s e r p o s (l) + l i f t serp>os(3) = se rp o s(3 ) + ( l i f t serp>os(4) = se rp o s(4 ) - s tr id e serp> os(6 ) = s e r p o s ( ) - s tr id e s e r p i o s (7) = se rp o s(7 ) - l i f t = se rp o s(9 ) - ( l i f t serp>os(9) serp io s (1 ) gosuib pause = s e r p o s ( ) + / 2) / 2) s tr id e sout 0 gosuib homepos gosuib so u t gosuib s _ lltu gosuib so u t go to 'm ax l i f t serp io s (1 ) gosuib 65 rm antis hom epois: 'A lte r th e se serp io s (0 ) = 127 ' 127 r ig h t to s u ite fro n t H V your ro b o t (+ forw ard) (+ l i f t ) serp io s (1 ) = 65 ' rig h t fro n t serp>os (2 ) = 127 ' 127 rig h t m iddle H (+ forw ard) s e r p i o s (3) = 70 '0 r ig h t m iddle V (+ l i f t ) s e r p > o s (4 ) = 125 ' 127 rig h t re a r H (+ forw ard) l i f t ) s e r p i o s (5) = 65 ' r ig h t re a r V (+ serpios (6 ) = ' 127 l e f t fro n t H (- forw ard) s e r p i o s (7) = 180 '2 l e f t fro n t V (- l i f t ) se rp io s (8 ) = 127 ' 127 l e f t m iddle H (- forw ard) s e r p > o s (9) = 0 '2 l e f t m iddle V (- l i f t ) se rp io s (1 ) = 127 ' 127 l e f t re a r H (- forw ard) serpios (1 ) = 195 '2 l e f t re a r V (- l i f t ) gosub sout retu rn i forw ar-d: gosuib s 0_ 10hf gosuib s _ lltd gosuib so u t gosuib s l_ tu gosuib s 0_ hb gosuib so ut g o su b s 0_ 10 hb g o su b s l_ td g o su b sout g o su b s _ lltu g o su b s 0_ h f g o su b so u t r e t u r n b a c k u p : s e r o u t fo r y '1 "B acking Up "16 "T urning L e ft "16 "W alking Forw ard "] "16 " , "T urning R ig h t "] ", "W alk in g Forw ard "] , $ C ,[16, = to ste p s g o su b s 0_ hb g o su b s _ lltd g o su b sout g o su b s l_ tu g o su b s 0_ 10 h f g o su b so u t g o su b s 0_ 10 h f g o su b s l_ td g o su b sout g o su b s _ lltu g o su b s 0_ hb g o su b so u t next: tu rn _ l.e f t : s e r o u t 13, $ 18C, [16, m an eu v er fo r y = = to goisub m aneuver +1 ste p s s 0_ tr n l g o su b s _ lltd g o su b so u t g o su b s l_ tu g o su b s 0_ tr n lb gO)sub so u t g o su b s 0_ tr n lb g o su b s l_ td g o su b so u t g o su b s _ lltu g o su b s 0_ tr n l g o su b so u t n ex t s e r o u t , $418C, [16, re tu rn i tu rn _ _ r:ig h t: s e r o u t ,$ C ,[16, m an eu v er fo r y = = m aneuver to +1 s te p s g o su b s 0_ tr n lb gojsub s _ lltd g o su b so u t go>sub s l_ tu g o su b s 0_ tr n l gojsub so u t g o su b s 0_ tr n l g o su b s l_ td g o su b so u t go>sub s _ lltu gojsub s 0_ tr n lb g o su b so u t n ex t s e r o u t re tu rn 13, $418C, [16, "16 s o u t : fo r x = to s e e ro u t 11 ,$4 , [ 5 , x , s e r p o s ( x ) ] n e x tt p ausse d elay re tu rn n s0 _ h h f: 'H o riz o n ta l Forw ard serppos(O ) = s e r p o s (0 ) - s t r i d e s e rp p o s (2 ) = s e r p o s (2 ) + s t r i d e serp p o s(4 ) = s e r p o s (4) s e r p p o s (6 ) = s e r p o s (6 ) - s tr id e s e rp p o s (8 ) - s tr id e = s e r p o s (8 ) + s t r i d e serp p o s(lO ) = s e r p o s (10) - s tr id e re tu rn n s _ llttd : 'T rip o d Down serp p o s(3 ) = s e r p o s (3) - l i f t serp p o s(7 ) = s e r p o s (7) + l i f t s e rp p o s (ll) = s e r p o s (1 ) + l i f t re tu rn n sl_ tu u : 'T rip o d Up s e r p p o s (1 ) s e r p o s (1 ) + l i f t s e r p p o s (5) s e r p o s (5) + l i f t s e r p p o s (9) s e r p o s (9) - l i f t re tu rn n s 0_ l 0h h b : 'H o riz o n ta l Back s e r p p o s (0 ) s e r p o s (0 ) -t s tr id e s e r p p o s (2 ) s e r p o s (2 ) - s tr id e s e r p o o s (4) s e r p o s (4) •+ s t r i d e s e r p o o s (6 ) s erp o s (6 ) -f s tr id e s e r p o o s (8 ) s e r p o s (8 ) - s tr id e s e r p p o s (1 ) = s e r p o s (1 ) + s tr id e re tu rn n s l_ td i: 'T rip o d Down s e r p p o s (1 ) = s e r p o s (1 ) - l i f t s e r p o o s (5) = s e r p o s (5) - l i f t s e r p o o s (9) = s e r p o s (9) + l i f t re tu rn n s _ llttu : 'T rip o d Up s e r p o o s (3) = s e r p o s (3) + l i f t s e r p o o s (7) = s e r p o s (7) - l i f t s e r p o o s (1 ) = s e r p o s (1 ) - l i f t re tu rn n s 0_ t t r n l : 'T rip o d ? s e r p o o s (0 ) s e r p o s (0 ) s tr id e s e r p o o s (2 ) s e r p o s (2 ) s tr id e s e r p o o s (4) s e r p o s (4) s tr id e s e r p o o s (6 ) s e r p o s (6 ) s tr id e s e r p o o s (8 ) s e r p o s (8 ) s tr id e s e r p o o s (1 ) = s e r p o s (1 ) + s t r i d e re tu rn s s 0_ 10t t r n l b : 'T rip o d ? s e r p o o s (0 ) = s e r p o s (0 ) + s tr id e s e rp o o s (2 ) = s e r p o s (2 ) - s tr id e s e r p o o s (4) = s e r p o s (4) + s tr id e s e r p o o s (6 ) = s e r p o s (6 ) - s tr id e s e r p o o s (8 ) = s e r p o s (8 ) + s tr id e s e r p o o s ( )i = s e r p o s (1 ) - stri< ret u r n ' { $STAiM P BS2E } ' H E BSE 'T h is is 'D e fin e th e RAM R em ote C o n tro l used serpos; v ar b y t e (12 l i f t v ar b y te strid e? v ar b y te d e la y v ar w ord m an eu v er v a r n ib ste p s v a r n ib tem pi v a r n ib tem pr v a r n ib X v a r n ib y v a r n ib z v ar n ib a v a r n ib b v ar w ord rex v ar w ord rcy v ar w ord gosub homepos gosub so u t s e ro u t ,$ C ,[1 ,"O p e tin g Under R em ote C o n tro l s tic k p o s itio n "1 ste p s= s t a r t : Xh r e x p u l s ini 14 , p u l s ini 15,1 „ rc y debug " re x " , 'G e t dec rex, your c r, "rc y ", i f rc y '< fihen s u b _ tu rn _ le ft i f rcy> 827 fzhen s u b _ tu rn _ rig h t i f rcx< 687 tthen sub_forw ard i f rcx.> 827 t:hen sub_backup i f m am euver = gosub s3 _ llt:d gosub sout th en dec rcy , 'M a k e from th e RC re c e iv e r d e c isio n s on th e c r some b e h av io u r, s t a r t 'p re p a re th re e le g s to f in is h w alking c y cle, s t a r t : m aneu ver goto = 'b o r r o w e d th is v a r ia b le to know s t a r t 'A lte r hom epojs: = '1 s e r p s o s (1) = '0 s e r p s o s (2) = 127 '1 s e r p s o s (3) = 70 ' s e r p o o s (4 ) = 125 s e r p « o s (5) = 65 serpcos (6 ) = s e r p s o s (7) s e r p s o s (8 ) s e r p s o s (9) s e r p s o s (0) th e se r ig h t to su i te your robot fro n t H (+ forw ard) r ig h t fro n t V r ig h t m iddle H (+ (+ l i f t ) forw ard) r ig h t m iddle V (+ l i f t ) ' 127 r ig h t re a r H (+ ' r ig h t re a r V (+ l i f t ) '1 l e f t fro n t H (- forw ard) = 180 '2 l e f t fro n t V (- l i f t ) = 127 '1 l e f t m iddle H (- forw ard) = 0 '2 l e f t m iddle V (- l i f t ) forw ard) s e r p s o s (10) = 127 '1 l e f t re a r H (- forw ard) serp o o s(ll) = 195 '2 l e f t re a r V (- l i f t ) gosuib re tu rn so u t i f ro b o t is sto p p ed fo rw a rrd : i f m anneuver = th en gosub s _ l l t u 1p r e p a r e gosub so u t fo rw ard th re e leg s to b e g in w alking c y cle leg s to b e g in w alking c y cle to b e g in w alking cy cle fo rw a rrd : m aneeuver fo r y = goosub = to ste p s s 0_ h f goosub s _ lltd goosub sout goosub s l_ tu goosub s 0_ 10hb goosub sout goosub s 0_ hb goosub s l_ td goosub sout goosub s _ lltu goosub s 0_ 10hf goosub sout n e x tt re tu u rn backupp: i f m anneuver = th en gosub s _ lltu 'p r e p a r e gosub so u t backup2 th re e backupp2 : m aneeuver fo r y = = to ste p s goosub s 0_ hb goosub s3 lltd goosub sout goosub s l_ tu goosub s 0_ 10hf goosub sout goosub s 0_ 10hf goosub s l_ td goosub sout goosub s _ lltu goosub s 0_ hb goosub sout n e x tt re tu u rn t u r n _ l i e f t : i f m anneuver = th en gosub s _ lltu 'p r e p a r e gosub sout t u r n _ l l e f t : m aneeuver fo r y = = 1 to s te p s goosub s 0_ 10 t r n l goosub s _ lltd goosub sout goosub s l_ tu goosub s 0_ 10 tr n lb goosub sout goosub s 0_ t r n l b goosub s l_ td goosub so u t goosub s _ lltu goosub s 0_ tr n l goosub sout n e x tt re tu rn n t u r n _ l e f t th re e le g s t u r n _ i r i g h t : i f m anneuver = th en tu r n _ r ig h t2 g o s u b > s _ _ l l t u ’p r e p a r e g o su b 't th re e le g s to so u t turn i r i g h t : m aneeuver fo r y = = to s te p s gcosub sO lO trn lb gcosub s _ lltd gcosub sout gcosub s l_ tu goosub s 0_ tr n l gcosub so u t gcosub s t r n l gcosub sl 9td goosub so u t goosub s3 goosub s t r n l b goosub so u t l l t u n e x tt re tu rn n su b _ fo o rw a rd : gosuub go to forw ard s s t a r t sub_baackup: gosuub goto backup s s t a r t s u b _ tu u r n _ le f t: gosuub g oto tu r n _ le f t s s t a r t s u b _ tu u rn _ rig h t: gosuub go to tu rn _ r ig h t s s t a r t s o u t : fo r X = to s e ro u t n ex t 11 ,$ ,[2 5 ,X ,s e rp o s (x )] t p a u sse d e la y re tu rn q ’H o r iz o n ta l s 0_ 10h n f : Forw ard s e r p o o s (0 ) : serpos(O ) - s e r p o o s (2 ) : s e r p o s ( ) + s tr id e s tr id e s e r p o o s (4 ) : se rp o s(4 ) - s tr id e s e r p o o s ( ) : s e r p o s ( ) - s tr id e s e r p o o s (8 ) s s e r p o s ( ) + s tr id e s e r p o o s (1 ) = serp o s(lO ) - s tr id e re tu rn s _ lltc d : ’T r i p o d Down se rp o o s(3 ) = se rp o s(3 ) - se rp o o s(7 ) = se rp o s(7 ) + s e r p o o s ( ll) re tu rn = s e r p o s ( ll) l i f t l i f t + l i f t i s l_ tu a : ’T r i p o d Up s e rp o o s (l) = s e r p o s (l) + l i f t se rp o o s(5 ) = s e rp o s(5 ) + l i f t se rp o o s(9 ) = s e rp o s(9 ) - l i f t beg in w alking cy cle returrn s 0_ b h b : 'H o riz o n ta l Back s e rp p o s ;0 ) : s e r p o s (0 ) + s tr id e s e rp p o s ;2 ) = s e r p o s (2 ) - s tr id e ■ s e r p o s (4) + s tr id e s e rp p o s !6 ) : s e r p o s (6 ) + s tr id e s e r p p o s (8 ) ; s e r p o s (8 ) - A) serp p o s s tr id e = s e r p o s (1 ) s e r p p o s (1 ) + s tr id e re tu rr n s l_ tc d : 'T rip o d Down s e r p p o s (1 ) =serp o s (1 ) - l i f t s e r p p o s (5) = s e r p o s (5) - l i f t s e r p p o s (9) = s e r p o s (9) + l i f t re tu rn n s _ l l t t u : 'T rip o d Up s e r p p o s (3) = s e r p o s (3) + s e r p p o s (7) = s e r p o s (7) - s e r p p o s (1 ) = l i f t l i f t s e r p o s (1 ) - l i f t re tu rn n s 0_ l t t r n l : 'T rip o d ? s e r p p o s (0 ) s e r p o s (0 ) s tr id e s e r p o o s (2 ) s e r p o s (2 ) s tr id e s e r p o o s (4 ) s e r p o s (4) s tr id e s e r p o o s (6 ) s e r p o s (6 ) s tr id e s e r p o o s (8 ) s e r p o s (8 ) s tr id e s e r p o o s (1 ) = + s e r p o s (1 ) s tr id e re tu rn n s 0_ t t r n l b : 'T rip o d ? s e r p o o s (0 ) = s e r p o s (0 ) h s tr id e s e r p o o s (2 ) = s e r p o s (2 ) - s tr id e s e r p o o s (4) = s e r p o s (4) H s tr id e s e r p o o s (6 ) = s e r p o s (6 ) - s tr id e s e r p o o s (8 ) = s e r p o s (8 ) h s e r p o o s (10 ) = s tr id e - s e r p o s (1 ) s tr id e re tu rn i ' { $STAKMP BS2E} ' H E BSE 'T h is [program 'S S C -1 2 is T his fo r ta b le th e is Hexapod to h e lp w ith se tu p a th e ro b o t Next Step u sin g ' Srv » ID '0 FRFH R ig h t F ront H o riz o n ta l (+ leg forw ard) ' FRFV R ig h t F ront V e rtic a l (+ leg l i f t ) '2 F R MH R ig ht M iddle H o riz o n ta l (+ leg forw ard) A ctio n e sc rip tio n Home '3 FRMV R ight M iddle V e rtic a l (+ le g l i f t ) ' FRRH R ight R ear H o riz o n ta l (+ le g forw ard) R ear '5 FRRV R ig ht ' I LFH L e ft '7 ILFV '8 ILMH '9 ILMV '1 '1 M icro, H itec 127 50 127 50 127 V e rtic a l (+ le g l i f t ) F ro n t H o riz o n ta l (- le g forw ard) 127 L e ft F ro n t V e rtic a l (- leg l i f t ) 205 L e ft M iddle H o riz o n ta l (- leg forw ard) 127 L e ft M iddle V e rtic a l (- leg l i f t ) 205 I LRH L e ft R ear H o riz o n ta l (- leg forw ard) 127 ILRV L e ft R ear V e rtic a l (- leg l i f t ) 205 'S ta m p I/O 'S ta m p I/O 12 to to SSC s e r i a l IRPD in p u t S ensor o u u t 50 IRPD serv o s and th e ' Staram p I/O 11 to IRPD R ight 'S ta m p I/O 10 to IRPD L eft 'S ta m p I/O p in s 0-3 'S ta m p I/O p in s 13 'S ta m p I/O p in s 4-7 'S ta m p i I/O p in s 9, IR IR re se rv e d re serv ed fo r fo r re se rv e d 14, 15 LED LED se rv o s LCD fo r or bumper sw itch es d is p la y B u tto n s and L E D 's in p u t, fre e 'ALL Li VARIABLES 'd i r r s = x b y te v a a r a c tid o n %1110111111111111 v a r v a r b it tem p p r v a r b it ' DEFFINE SSC-12 p in = CHANNELS rfv con rmv rrh rrv lfh 1f V lm h lm v lrh 10 lrv 11 'S E T T LIM ITS ' le g j m o tion FOR / fro n t rig h tit m iddle LEG MOTION down down id e a l 75 '7 77 '7 rig h h t rear_dow n 73 ' 70 rig h tit fro n t_ u p '2 rig h h t_ m id d le _ u p 21 '2 rightit rear up 208 '2 le ft_ t_ fro n t down 173 '1 e f t t _ m i d d 1e_dow n 182 ' 185 l e f t t _ r e a r down 178 '1 up '5 38 l e f t t_m i d d 1e_up 47 '5 l e f t t _ r e a r 43 '5 rig h tit_ f ro n t 157 rig h tit back 97 '1 »97 le ft_ t_ f ro n t 97 ' 97 l e f t t_back 157 '1 132 ' (+ 5) S et th e se 134 ' (+7) use th e 130 ( + 3) These d i f f e r e n t le ft_ t_ fro n t up fro n t hor h o r ii z o n ta l, rig h h t w ill hor l e f t t_ fro n t_ h o r 115 ( - ) 124 ' (-3) 120 (-7) hor l e f t t_ re a r_ h o r GAIT 32 '3 d e la a y 70 '8 50 d e la a y 60 '7 40 THE v a lu e s INFO slow w 'SET T d e lta be le f tt_ m id d le 'OTHHER make th e serv o horns from 127 to s e t th e p ro p er v a lu e s rig h h t_ _ rear_ h o r Y ourrs to th en m iddle l rfh 'I/O LEGS INTO HORIZONTAL 48 PO SITIO N, WAIT SECOND w ere fo r th e p ro to ty p e robot ' Re.em ove th e rem s to fin d th e d e lta u sed above ' s e t t _ h o r i z o n t a l : ' s s e r o u t ,$ ,[2 5 , rfh , 0, 255, rm h, 0, 255, rrh , 0] ' s s e ro u t 8,$4054, lfh , 0, 255, lm h, 0, 255, lrh , 0] r ig h t_ f r o n t_ h o r ] [255, ' s s e r o u t ,$ ,[2 5 , rfv , ' s s e r o u t ,$ ,[2 5 , rm v, rig h t_ m id d le _ h o r] ' s s e r o u t ,$ ,[2 5 , rrv , r ig h t_ r e a r _ h o r ] s s e r o u t ,$ ,[2 5 , lfv , l e f t_ f r o n t_ h o r ] ' s s e r o u t 8,$4054, lm v, le ft_ m id d le _ h o r] ' s s e r o u t ,$ ,[2 5 , lrv , le f t_ r e a r _ h o r ] 'p a u u s e 'g o t to 0 s e t_ h o r iz o n ta l THE p a u ss e LEGS INTO HOME POSITION, WAIT SECOND 0 [255, rfv , r ig h t fro n t [255, rm v, r ig h t m id d le [255, rrv , r ig h t_ r e a r [255, lfv , e f t _ f ro n t_ d o w n ] ,$ ,[ 5 , lm v, l e f t m id d le lrv , l e f t re a r WHAT THE ,$ , ,$ , se e ro u t co se e ro u t se e ro u t ,$ , to o p a u s'se o cn se e ro u t ■co­ se e ro u t CD se e ro u t -0 > ’SETT [255, [255, down] down] down] down] down] 0 'CHE