(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm

(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm(Luận văn thạc sĩ) Thiết kế, chế tạo mô hình tay máy gắp sản phẩm

MỤC LỤC Trang Nội dung CHƯƠNG I : DẪN NHẬP 1.1 Đặt vấn đề 1.2 Tầm quan trọng đề tài 1.3 Điểm đề tài 1.4 Giới hạn đề tài 1.5 Mục đích nghiên cứu 1.6 Phương pháp nghiên cứu 1.7 Thời gian nghiên cứu 1 2 3 chương : Cơ sở lý luận 2.1 Sự xuất thuật ngữ robot tình hình nghiên cứu sử dụng robot nước giới 2.2 Tình hình phát triển robot Việt Nam 2.3 Nhu caàu thực tế vấn đề sử dụng robot nghiệp CNH-HĐH nước ta 2.4 Những công việc đòi hỏi nên đưa vào sử dụng cấu robot 2.5 Các loại robot công nghiệp phạm vi ứng dụng loại 2.5.1 Robot Scara bậc tự 2.5.2 Robot Puma 10 2.5.3 Robot Stanford 10 2.5.4 Một số khó khăn tính toán chế tạo robot nước ta giai đoạn 11 2.6 Cơ sở lý thuyết ma trận chuyển, phương trình robot 12 2.6.1 Ma trận chuyển đổi tịnh tiến hai toạ độ 12 2.6.2 Ma trận biến đổi xoay hai toạ độ 13 2.7 Ma trận biến đổi xoay quay trục đặc biệt 15 2.7.1 Ma trận biến đổi xoay hệ trục toạ độ OX1Y1Z1 quanh trục X góc α so với hệ trục toạ độ OXYZ 15 2.7.2 Ma trận biến đổi xoay hệ trục toạ độ OX1Y1Z1 quanh trục Y góc β so với hệ trục toạ độ OXYZ 16 2.7.3 Ma trận biến đổi xoay hệ trục toạ độ OX1Y1Z1 quanh trục Z góc θ so với hệ trục toạ độ OXYZ 16 2.7.4 Bộ thông số DH(Denavit J & Hartenberg R.S) 17 2.7.5 Thiết lập hệ trục toạ độ 18 2.7.6 Mô hình biến đổi 19 2.7.7 Phương trình động học 19 2.8 Cô sở lý thuyết ma trận Jacobi 20 2.9 Phương trình Lagarange bậc II ứng dụng phân tích động lực hoïc cho robot 23 2.10 Cô sở lý thuyết họ vi xử lý 8051 24 2.10.1 Giới thieäu chung 24 2.10.2 Cấu Trúc AT89S52 25 2.10.3 Sơ đồ khối 26 2.10.4 Sơ đồ chân 27 2.10.5 Caáu trúc vùng Ram 28 2.10.6 Ngôn ngữ điều khieån 28 2.10.7 Kết nối với máy tính 29 2.10.8 Cấu trúc cổng xuất nhập mô tả chức chân vi điều khiển .33 2.11 Giới thiệu cấu tạo nguyên lý hoạt động động bước 36 2.11.1 Nguyên tắc hoạt động chung động bước 36 2.11.2 Nguyên tắc họat động động bước dạng từ dẫn thay đổi 37 2.11.3 Điều khiển động bước 41 2.12 Cơ sở lý thuyết giao tiếp máy tính 42 2.12.1.Các phương pháp giao tiếp 42 2.12.2.Các hình thức giao tiếp máy tính 43 2.12.3 Tiêu chuẩn giao tiếp cổng nối tiếp 43 2.12.4 Chức chân cổng giao tiếp 43 2.12.5 Phương pháp kết nối 44 2.12.6 Nguyên lý truyền liệu 46 2.13 Khaùi quaùt giao tiếp nối tiếp matlab 46 2.14 Trình tự thực giao tiếp thiết bị với cổng nối tiếp 47 Chương III: Chọn sơ đồ cấu, tính toán động học động lực học cho robot bậc tự 49 3.1 Chọn sơ đồ kết cấu cho robot bậc tự 49 3.2 Các kích thước ban đầu 50 3.3 Tính toán động học thuận cho robot bậc tự 50 3.3.1 Nhiệm vụ 50 3.3.2 Sơ đồ robot gắp sản phẩm, thông số DH 51 3.3.3 Phân tích động học thuận cho robot 51 3.4 Phaân tích động học ngược cho robot 55 3.5 Ứng dụng ma trận Jacobi quay vít để tính toán vận tốc cho khớp tác động cuối 60 3.6 Tính toán động lực học cho robot bậc tự 65 3.6.1 Tính bền cho toàn cấu dựa sở tónh học 65 3.6.2 Kiểm tra động lực học cho tay máy thực nhiệm vụ 72 Chương IV: Thiết kế điều khiển chương trình giao tiếp máy tính 78 4.1 Phân tích điều khiển tay maùy 78 4.2 Thiết kế mạch điện cho giải mã 80 4.2.1: Nhiệm vụ 80 4.2.2 Sơ đồ mạch điện điều khiển vi bước 80 4.3 Thiết kế mạch điều khiển 81 4.3.1.Nhiệm vụ 81 4.3.2 Thiết kế mạch điều khiển 81 4.4 Thiết kế mạch xử lý tín hiệu từ cảm bieán 82 4.4.1 Nhiệm vụ 82 4.4.2 Sơ đồ mạch điện xử lý tín hiệu cảm biến 82 4.5 Thieát keá giao diện điều khiển 83 4.5.1 Nhiệm vụ 83 4.5.2 Xử lý số liệu nhập 84 4.5.3 Giao diện điều khiển 84 4.6 Trình tự lập trình hoạt động cho tay máy 85 4.7 Mô hoạt động 85 4.7.1 Giới thiệu phần meàm Solid edge 85 4.7.2 Giới thiệu phần mềm Visual Natran 86 4.8 Thuật toán điều khiển cho robot gắp sản phẩm 87 Chương V: Thực Nghiệm 88 5.1 Chế tạo giải mã 88 5.1.1 Sơ đồ mạch in 88 5.1.2 Sơ đồ thực tế 88 5.2 Chế tạo điều khiển trung tâm, Bộ điều khiển trung gian card giao tiếp 89 5.2.1 Sơ đồ mạch in 89 5.2.2 Sơ đồ thực tế 89 5.3 Chế tạo mạch cảm biến 90 5.3.1 Sơ đồ mạch in 90 5.3.1 Sơ đồ thực tế 90 5.4 Cấu tạo điều khiển mô hình thực tế 90 5.5 Giải toán thực tế 92 Chương VI: Kết luận hướng phát triển 96 6.1 Kết luận 96 6.2 Hướng phát triển đề tài 96 Phụ lục Chương trình viết cho điều khiển trung tâm điều khiển trung gian 97 Chương trình viết cho điều khiển trung tâm 97 Chương trình viết cho điều khiển trung gian 102 Chương trình viết cho điều khiển trung gian 113 Phuï luïc 2: Thuật toán điều khiển động vi bước, chương trình viết cho vi bước hợp ngữ chương trình matlab tính toán liệu vào 117 Thuật toán điều khiển vi bước 117 Chương trình điều khiển vi bước 117 Chương trình tạo giao diện tính toán liệu vào 123 Phụ lục 3: Các hệ số phương trình động lực học cho robot bậc tự 134 Tài liệu tham khảo 141 LUẬN VĂN THẠC SỸ KỸ SƯ : NGUYỄN VĂN ĐOÀN CHƯƠNG I DẪN NHẬP 1.1 Đặt vấn đề Hiện tình hình robot giới đỉnh cao phát triển, việc ứng dụng robot nhiều lónh vực khác đời sống thể rõ rệt Nhu cầu thay dần công việc người tay máy có chức tương tự tiếp tục thực hiện, Trong đó, tiếp cận lónh vực Việt Nam tính đến nhiều hạn chế, thiếu số lượng chất lượng sản phẩm robot thực tế, thiếu tài liệu tính toán, thiết bị điều khiển công nghệ chế tạo sản phẩm robot hoàn chỉnh Vì vậy, để tăng tốc độ tiếp cận áp dụng rộng rãi công nghệ cần phải đẩy mạnh tốc độ phát triển vấn đề tính toán công nghệ chế tạo thiết bị đại này, mà trước hết phải triển khai phát triển đề tài nước làm tiền đề cho phát triển sản phẩm thực tế sau Đứng trước tình hình nhiệm vụ trung tâm nghiên cứu, trường Đại học, cao đẳng nước vấn đề này, phải đặt lên tầm quan trọng hàng đầu, trường phải có nhiều đề tài nghiên cứu lónh vực thiết kế chế tạo, làm sở đào tạo cán kỹ thuật có lực, đáp ứng nhu cầu phát triển ngành robot tương lai gần Trên sở đó, đề tài “Thiết kế, chế tạo tay máy gắp sản phẩm”, với trình bày cách tổng quát trình tự thiết kế, chế tạo robot hoàn chỉnh bậc tự do, đề tài có ý nghóa cho trình đào tạo lónh vực robot Việt Nam, đồng thời tiền đề cho nhà máy, xí nghiệp thực trình sản xuất tay máy ứng dụng vào thực tế đời sống 1.2 Tầm quan trọng đề tài Sự thay dần sức người sức máy vấn đề quan trọng trình đất nước chuẩn bị hoàn thành giai đoạn CNH-HĐH tiến lên chủ nghóa xã hội, với đề tài “Thiết kế, chế tạo tay máy gắp sản phẩm” làm sáng tỏ vấn đề mối liên quan lónh vực Công nghệ thông tin – Điện tử – Cơ khí mà thể mức độ rút ngắn khoảng cách tiếp cận công nghệ Việt Nam so với nước giới, tạo đòn bẩy thúc đẩy phát triển nhanh lónh vực robot nước ta Với kết cấu bậc tự do, cấu tay máy linh hoạt, điểm tác động cuối chuyển động theo quỹ đạo quay theo trục nằm vùng không gian hoạt động Điều có ý nghóa kiểm tra thể TRANG LUẬN VĂN THẠC SỸ KỸ SƯ : NGUYỄN VĂN ĐOÀN giải thuật điều khiển từ máy tính Đồng thời không phần quan trọng để sinh viên thử nghiệm viết chương trình điều khiển quỹ đạo robot theo ý muốn Nếu dây truyền sản xuất tự động, có trang bị loại robot linh hoạt này, có yêu cầu thay đổi chu trình làm việc, thay phải thay cấu robot khác, cấu robot cần nạp vào chương trình điều khiển đáp ứng Điều giúp nhà máy giảm chi phí thời gian, tiền nắm bắt thời chu kỳ phát triển sản phẩm 1.3 Điểm đề tài Phần tính toán lý thuyết động học, động lực học thể đề tài, so với đề tài trước đây, chủ yếu dựa sở lý thuyết tính toán tài liệu có, lý thuyết vi xử lý nói riêng kỹ thuật điện tử nói chung chủ yếu mang tính ứng dụng vào thực tế Tuy nhiên, với phần lập trình điều khiển, người nghiên cứu sử dụng trực tiếp công cụ tính toán phần mềm Matlab để truyền tải liệu cho khối vi xử lý, khối vi xử lý trực tiếp điều khiển cấu chấp hành Có thể nhấn mạnh rằng, điểm đề tài : người nghiên cứu tổng hợp nối kết mảng kiến thức lónh vực : Công nghệ thông tin – Điện tử – Cơ khí, để tạo nên sản phẩm robot hoàn chỉnh Ngoài ra, với ứng dụng phần mềm : Autocad, Inventer, Solid egle, Visual nastran vào việc vẽ mô làm cho vấn đề lý thuyết sáng tỏ hơn, thêm vào công cụ tạo giao diện đơn giản hoá trình thiết lập quy luật điều khiển cho tay máy, điều giảm thiểu thời gian cho người sử dụng độc giả có quan tâm 1.4 Giới hạn đề tài Do thời gian nghiên cứu điều kiện vật chất có hạn nên đề tài người nghiên cứu tính toán, thiết kế chế tạo thử nghiệm mô hình robot bậc tự Mô hình nâng vật nặng tối đa 1kg, vận tốc tối đa 0.05m/s, tác động cuối phận gắp chi tiết dạng hộp có kích thước tối đa 60 x 60 x 60 tối thiểu 30 x 30 x 30 ứng dụng chủ yếu công tác đào tạo trường Đại Học, Cao Đẳng Trung Học Trong nước 1.5 Mục đích nghiên cứu Với đề tài ““Thiết kế, chế tạo tay máy gắp sản phẩm” người nghiên cứu thể với mục đích sau: TRANG LUẬN VĂN THẠC SỸ KỸ SƯ : NGUYỄN VĂN ĐOÀN - Hoàn thiện lượng kiến thức lớn tin học điện tử nói chung, công nghệ tự động hoá nói riêng - Giới thiệu cho học sinh trường kỹ thuật sản phẩm hoàn chỉnh tay máy với bậc tự bao gồm phần tính toán học, thiết kế mạch điều khiển, chương trình điều khiển 1.6 Góp phần phát triển nghành công nghệ tự động nước nhà Phương pháp nghiên cứu - Phương pháp tham khảo tài liệu : Tài liệu robot công nghiệp, tài liệu thiết kế mô phỏng, tài liệu lập trình ngôn ngữ matlab, tài liệu động bước, động Servo, tài liệu vi xử lý linh kiện bán dẫn, linh kiện điện tử công suất - Phương pháp thực nghiệm : Lắp ráp thí nghiệm mạch điều khiển động bước, Bộ điều khiển chính, cổng truyền thông nối tiếp với phần mềm matlab, gia công toàn chi tiết lắp ráp hoàn chỉnh mô hình robot bậc tự 1.7 Thời gian nghiên cứu - Từ tháng 2/2006 đến tháng 3/2006, tham khảo tài liệu tính toán thông số để thiết kế chế tạo mô hình robot - Từ tháng 3/2006 đến tháng 6/2006, thiết kế chế tạo mô hình robot bâc tự - Từ tháng 6/2006 đến tháng 8/2006, thử nghiệm chế tạo điều khiển, Card giao tiếp với phần mềm matlab - Từ tháng 8/2006 đến 15/9/2006,Tổng kết hoàn thành thuyết minh TRANG LUẬN VĂN THẠC SỸ KỸ SƯ : NGUYỄN VĂN ĐOÀN CHƯƠNG II CƠ SỞ LÝ LUẬN 2.1 Sự xuất thuật ngữ robot tình hình nghiên cứu sử dụng robot nước giới Thuật ngữ “robot” bắt đầu xuất vào khoảng năm 1922 tác phẩm “Rossum’s Universal Robot” Karel Capek Theo tiếng séc có nghóa người tạp dịch Trong tác phẩm nhân vật Rossum trai ông ta tạo máy gần giống người để hầu hạ người Hơn 20 năm sau, ước mơ viễn tưởng Karel Capek bắt đầu trở thành thực Ngay sau chiến tranh giới lần thứ 2, Hoa Kỳ xuất tay máy chép hình điểu khiển từ xa phòng thí nghiệm vật liệu phóng xạ Vào năm 50 hàng loạt tay máy dạng chép hình xuất đến năm 1975 công ty Unimation bắt đầu có lợi nhuận từ sản phẩm robot Khái niệm robot công nghiệp đời đưa vào ứng dụng vào năm 1961 hoa kỳ Năm 1967 nhật nhập robot công nghiệp từ công ty AMF hoa kỳ, đến năm 1990 có 40 công ty nhật bản, có công ty khổng lồ công ty Hitachi công ty Mitsubishi, đưa thị trường quốc tế nhiều loại robot tiếng Từ sau việc đưa vào ứng dụng loại robot phát triển mạch nhật, mỹ nghiêng phần tính toán sở lý thuyết Từ năm 70 việc nghiên cứu nâng cao tính robot ý nhiều đến lắp đặt thêm cảm biến ngoại tín hiệu để nhận biết môi trường làm việc, robot có vài chức nhận biết giống người Tại trường đại học tổng hợp Stanford người ta tạo loại robot lắp ráp tự động điều khiển máy vi tính sở xử lý thông tin từ cảm biến lực thị giác Cũng vào thời gian công ty IBM chế tạo loại robot có cảm biến xúc giác cảm biến lực, điều khiển máy vi tính để lắp ráp máy in gồm 20 cụm chi tiết Từ năm 80 vào khoảng năm 90 áp dụng rộng rãi tiến vi xử lý công nghệ thông tin, số lượng robot công nghiệp gia tăng, giá thành giảm rõ rệt so với trước tính phát triển vượt bậc, thay toàn công việc nặng nhọc độc hại cho người TRANG LUẬN VĂN THẠC SỸ KỸ SƯ : NGUYỄN VĂN ĐOÀN Chính từ ứng dụng lên chuyên ngành khoa học robot “Robotics” trở thành lónh vực rộng rãi khoa học, bao gồm vấn đề động học, động lực học, tính toán điều khiển theo quỹ đạo, cảm biến tín hiệu điều khiển chuyển động v.v… Việc ứng dụng robot giới nghiên cứu phát triển mạnh nước : Pháp, Đức, Italia, Nhật, Anh, Mỹ đến lan rộng sang nước châu Á : Hàn Quốc, Singapo, Đài Loan Tuy nhiên, năm 1991 việc ứng dụng công nghệ robot nước Châu Á thấp đến năm 2000 mức độ sử dụng phát triển gần ngang với nước phát triển Robot ứng dụng nhiều nước giới công nghệ chế tạo lắp ráp xe lắp ráp số sản phẩm khác Cho đến công nghệ sử dụng robot nước phát triển giai đoạn phát triển cao nhất, từ công việc giúp việc nhà, trang trí, khuôn dỡ hàng hoá, lắp ráp kiểm tra sản phẩm, an ninh quốc phòng, thám hiểm v.v.v có tham gia robot Đặc biệt công việc nặng nhọc mang tính độc hại sử dụng robot phương pháp nâng cao hiệu Các công ty nhật nhờ phát triển với tốc độ nhanh nghành điện tử công nghệ thông tin họ giới thiệu sản phẩm robot với hình dạng tính giống hệt người, ví dụ robot leo cầu thang, vỗ tay, cúi chào khán giả v.vv Robot ứng dụng nhiều công việc khác nhau, đề cập, khái niệm robot công nghiệp điểm mấu chốt công nghệ chế tạo robot, thuật ngữ robot công nghiệp nhiều tác giả định nghóa khác theo ISO robot công nghiệp tay máy đa mục tiêu, có số bậc tự do, dễ dàng lập trình, điều khiển trợ động, dùng để tháo lắp phôi, dụng cụ vật dụng Do chương trình thao tác thay đổi nên thực nhiều nhiệm vụ đa dạng Nói cách đầy đủ hơn, Robot công nghiệp thiết bị tự động linh hoạt, bắt chước chức lao động công nghiệp người 2.2 Tình hình phát triển robot Việt Nam Đối với nước ta, việc đưa vào sử dụng robot nói nước chậm với mục tiêu tiếp cận kỹ thuật lónh vực này, nước triển khai đề tài nghiên cứu khoa học cấp nhà nước : đề tài 58.01.03 giai đoạn 81-85 đề tài 52B.03.01 giai đoạn 86-89 Kết nghiên cứu đề tài khong đáp ứng nhu cầu tiếp cận mà có ứng dụng ban đầu kỹ thuật bảo hộ lao động phục vụ công tác đào tạo cán kỹ thuật nước ta TRANG LUẬN VĂN THẠC SỸ KỸ SƯ : NGUYỄN VĂN ĐOÀN Trong giai đoạn từ năm 1990 nghành công nghiệp nước bắt đầu đổi Nhiều sở cần phải trang bị số hệ thống thiết bị chủ yếu nhập dây truyền từ nước phát triển để phục vụ cho số công việc sau đây: - Tháo lắp dụng cụ cho trung tâm gia công máy CNC - Lắp ráp linh kiện điện tử - Tháo sản phẩm máy ép nhựa tự động, có loại thao tác phối hợp với hệ thống giám sát, điều khiển camera - Hàn vỏ xe - Phun sơn, phủ bề mặt Từ tháng 4/98 nhà máy Rorze/Robotech bước vào hoạt động khu công nghiệp Nonura Hải Phòng Đây nhà máy Việt Nam chế tạo lắp ráp robot Đó loại robot có cấu trúc đơn giản xác sản xuất chất bán dẫn Nhà máy Rorze Robotech có vốn đầu tư 46 triệu đôla mỹ Nhật Bản đầu tư Trong năm gần Trung Tâm Nghiên Cứu Kỹ Thuật (NCKT) tự động hoá, trường Đại Học Bách Khoa (ĐHBK) – Hà Nội nghiên cứu tính toán thiết kế kiểu robot ký hiệu robot RP Robot RP thuộc loại robot sinh (Bắt trứơc cấu tay người) Sự khác biệt robot với kiểu robot sinh khác dùng cấu pantograph với trượt dẫn động làm mô đun chủ yếu cấu tay máy Trung tâm nghiên cứu chế tạo thử nghiệm robot với tên RPS-406 dùng để phun men, Robot RPS – 4102 dùng công nghệ bề mặt có bậc tự do, Robot SCA mini dùng để phục vụ cho trường tham khảo nghiên cứu, trung tâm xây dựng thư viện tính toán mô số loại robot có giới Một số trường đại học cao đẳng phía nam miền trung thời gian giai đoạn nghiên cứu lại thành robot có sẵn trung tâm nghiên cứu trường đại học tiếng giới Từ sau năm 2003 tình hình phát triển ứng dụng robot phía nam phát triển bước vượt bậc, trường chủ chốt Đại học Bách Khoa TP.HCM Đại học Sư Phạm Kỹ Thuật mở lónh vực hoàn toàn lónh vực điều khiển tự động thích hợp cho phát triển robot Trong năm gần để lónh vực robot nước nhà hàng năm phủ phát động thi robocon, thi sinh viên hưởng ứng tham gia tích cực Tuy nhiên, phát triển ý tưởng sáng tạo robot chưa sâu vào chế tạo tính toán kết cấu thực tế robot, nước phát triển thời gian họ chủ yếu nghiên cứu công nghệ ứng dụng TRANG 1/20*s1*(30*s234+180*s23))*conj(s1*(30*s234+180*s23))+(5012/3*c234*s5+615/2*c234 +1845*c23)*conj(c234*s5)+(41/4*c234*s5+3/2*c234+9*c23)*conj(30*c234+180*c23) D34 = (-6/5*(-c1*c2*s3-c1*s2*c3)*c4-6/5*(c1*s2*s3-c1*c2*c3)*s4)*conj(-c1*c23*s4c1*s23*c4)+(3/25*(-c1*c2*s3-c1*s2*c3)*c4+3/25*(c1*s2*s3-c1*c2*c3)*s4)*conj(30*c1*s23*c4-30*c1*c23*s4-180*c1*s23)+(-6/5*(-s1*c2*s3-s1*s2*c3)*c4-6/5*(s1*s2*s3s1*c2*c3)*s4)*conj(-s1*s23*c4-s1*c23*s4)+(3/25*(-s1*c2*s3s1*s2*c3)*c4+3/25*(s1*s2*s3-s1*c2*c3)*s4)*conj(-30*s1*s23*c4-30*s1*c23*s4180*s1*s23)+(-6/5*(-s2*s3+c2*c3)*c4-6/5*(-c2*s3-s2*c3)*s4)*conj(c23*c4s23*s4)+(3/25*(-s2*s3+c2*c3)*c4+3/25*(-c2*s3-s2*c3)*s4)*conj(30*c23*c430*s23*s4+180*c23)-(-540*c1*s234*s5-135*c1*s234)*conj(c1*s234*s5)-(9/2*c1*s234*s5-3/2*c1*s234)*conj(c1*(30*s234+180*s23))-(-540*s1*s234*s5135*s1*s234)*conj(s1*s234*s5)-(-9/2*s1*s234*s53/2*s1*s234)*conj(s1*(30*s234+180*s23))+(540*c234*s5+135*c234)*conj(c234*s5)+(9/2 *c234*s5+3/2*c234)*conj(30*c234+180*c23)-(-5012/3*c1*s234*s5615/2*c1*s234)*conj(c1*s234*s5)-(-41/4*c1*s234*s53/2*c1*s234)*conj(c1*(30*s234+180*s23))-(-5012/3*s1*s234*s5615/2*s1*s234)*conj(s1*s234*s5)-(-41/4*s1*s234*s53/2*s1*s234)*conj(s1*(30*s234+180*s23))+(5012/3*c234*s5+615/2*c234)*conj(c234*s5) +(41/4*c234*s5+3/2*c234)*conj(30*c234+180*c23) D35 = -(-540*s1*s5-540*(-(c1*c2*c3-c1*s2*s3)*c4-(-c1*c2*s3c1*s2*c3)*s4)*c5)*conj(c1*s234*s5)-(-9/2*s1*s5-9/2*(-(c1*c2*c3-c1*s2*s3)*c4-(c1*c2*s3-c1*s2*c3)*s4)*c5)*conj(c1*(30*s234+180*s23))-(540*c1*s5-540*(-(s1*c2*c3s1*s2*s3)*c4-(-s1*c2*s3-s1*s2*c3)*s4)*c5)*conj(s1*s234*s5)-(9/2*c1*s5-9/2*((s1*c2*c3-s1*s2*s3)*c4-(-s1*c2*s3-s1*s2*c3)*s4)*c5)*conj(s1*(30*s234+180*s23))540*(-(s2*c3+c2*s3)*c4-(-s2*s3+c2*c3)*s4)*c5*conj(c234*s5)-9/2*(-(s2*c3+c2*s3)*c4-(s2*s3+c2*c3)*s4)*c5*conj(30*c234+180*c23)-(5012/3*c1*c234*c55012/3*s1*s5)*conj(c1*s234*s5)-(41/4*c1*c234*c541/4*s1*s5)*conj(c1*(30*s234+180*s23))-(5012/3*c1*s5+5012/3*s1*c234*c5)*conj(s1*s234*s5)-(41/4*c1*s5+41/4*s1*c234*c5)*conj(s1*(30*s234+180*s23))+5012/3*s234*c5*conj(c234*s 5)+41/4*s234*c5*conj(30*c234+180*c23) D36 = D44 = (-6/5*(-c1*c2*s3-c1*s2*c3)*c4-6/5*(c1*s2*s3-c1*c2*c3)*s4)*conj((-c1*c2*s3c1*s2*c3)*c4+(c1*s2*s3-c1*c2*c3)*s4)+(3/25*(-c1*c2*s3-c1*s2*c3)*c4+3/25*(c1*s2*s3c1*c2*c3)*s4)*conj(30*(-c1*c2*s3-c1*s2*c3)*c4+30*(c1*s2*s3-c1*c2*c3)*s4)+(-6/5*(s1*c2*s3-s1*s2*c3)*c4-6/5*(s1*s2*s3-s1*c2*c3)*s4)*conj((-s1*c2*s3s1*s2*c3)*c4+(s1*s2*s3-s1*c2*c3)*s4)+(3/25*(-s1*c2*s3-s1*s2*c3)*c4+3/25*(s1*s2*s3s1*c2*c3)*s4)*conj(30*(-s1*c2*s3-s1*s2*c3)*c4+30*(s1*s2*s3-s1*c2*c3)*s4)+(-6/5*(- s2*s3+c2*c3)*c4-6/5*(-c2*s3-s2*c3)*s4)*conj((-s2*s3+c2*c3)*c4+(-c2*s3s2*c3)*s4)+(3/25*(-s2*s3+c2*c3)*c4+3/25*(-c2*s3-s2*c3)*s4)*conj(30*(s2*s3+c2*c3)*c4+30*(-c2*s3-s2*c3)*s4)-(-540*c1*s234*s5135*c1*s234)*conj(c1*s234*s5)-30*(-9/2*c1*s234*s5-3/2*c1*s234)*conj(c1*s234)-(540*s1*s234*s5-135*s1*s234)*conj(s1*s234*s5)-30*(-9/2*s1*s234*s53/2*s1*s234)*conj(s1*s234)+(540*c234*s5+135*c234)*conj(c234*s5)+30*(9/2*c234*s5+3 /2*c234)*conj(c234)-(-5012/3*c1*s234*s5-615/2*c1*s234)*conj(c1*s234*s5)-30*(41/4*c1*s234*s5-3/2*c1*s234)*conj(c1*s234)-(-5012/3*s1*s234*s5615/2*s1*s234)*conj(s1*s234*s5)-30*(-41/4*s1*s234*s53/2*s1*s234)*conj(s1*s234)+(5012/3*c234*s5+615/2*c234)*conj(c234*s5)+30*(41/4*c23 4*s5+3/2*c234)*conj(c234) D45 = -(-540*s1*s5-540*(-(c1*c2*c3-c1*s2*s3)*c4-(-c1*c2*s3c1*s2*c3)*s4)*c5)*conj(c1*s234*s5)-30*(-9/2*s1*s5-9/2*(-(c1*c2*c3-c1*s2*s3)*c4-(c1*c2*s3-c1*s2*c3)*s4)*c5)*conj(c1*s234)-(540*c1*s5-540*(-(s1*c2*c3-s1*s2*s3)*c4-(s1*c2*s3-s1*s2*c3)*s4)*c5)*conj(s1*s234*s5)-30*(9/2*c1*s5-9/2*(-(s1*c2*c3s1*s2*s3)*c4-(-s1*c2*s3-s1*s2*c3)*s4)*c5)*conj(s1*s234)-540*(-(s2*c3+c2*s3)*c4-(s2*s3+c2*c3)*s4)*c5*conj(c234*s5)-135*(-(s2*c3+c2*s3)*c4-(s2*s3+c2*c3)*s4)*c5*conj(c234)-(5012/3*c1*c234*c5-5012/3*s1*s5)*conj(c1*s234*s5)30*(41/4*c1*c234*c5-41/4*s1*s5)*conj(c1*s234)-(5012/3*c1*s5+5012/3*s1*c234*c5)*conj(s1*s234*s5)-30*(41/4*c1*s5+41/4*s1*c234*c5)*conj(s1*s234)+5012/3*s234*c5*conj(c234*s5)+615/2*s234 *c5*conj(c234) D46 =0 D55 = (-540*s1*s5-540*(-(c1*c2*c3-c1*s2*s3)*c4-(-c1*c2*s3-c1*s2*c3)*s4)*c5)*conj(-s1*s5-((c1*c2*c3-c1*s2*s3)*c4-(-c1*c2*s3-c1*s2*c3)*s4)*c5)+(540*c1*s5-540*(-(s1*c2*c3s1*s2*s3)*c4-(-s1*c2*s3-s1*s2*c3)*s4)*c5)*conj(c1*s5-(-(s1*c2*c3-s1*s2*s3)*c4-(s1*c2*s3-s1*s2*c3)*s4)*c5)+540*(-(s2*c3+c2*s3)*c4-(-s2*s3+c2*c3)*s4)*c5*conj(((s2*c3+c2*s3)*c4-(-s2*s3+c2*c3)*s4)*c5)+(5012/3*c1*c234*c55012/3*s1*s5)*conj(c1*c234*c5-s1*s5)+(-5012/3*c1*s5+5012/3*s1*c234*c5)*conj(c1*s5+s1*c234*c5)+5012/3*s234*c5*conj(s234*c5) D56 =0 D66 =0 clear all; clc; format short global k k=1; %=====figure=======% f=figure('Numbertitle','off', 'color',[0.5 0.8 0.8], 'menubar','none', 'Position',[20 20 700 300], 'name','DIEU KHIEN ROBOT', 'handlevisibility','on', 'resize','on', 'Windowstyle','modal');centerfig(f) % ===== TAO PHAN CHU THICH SO DIEM TAY MAY DI QUA =======% uicontrol('Style','text', 'BackgroundColor',[0.9 0.9 0.9], 'Position',[10 270 170 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[0.9 0.5 0], 'String','Nhập số vị trí (số điểm)'); % ===== TAO NOI DUNG SO DIEM YEU CAU =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[185 270 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String','1','tag','npoint'); % ===== TAO CHUONG NUT GOI CHAP NHAN CHUONG TRINH =======% uicontrol('Style','push', 'BackgroundColor',[0.8 0.85 0.8], 'Position',[245 270 100 25], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[0 0], 'String','ACCEPT', 'callback','nhapmatran'); function nhapmatran() global k % % ===== TAO PHAN CHU THICH thong so cho diem thu i =======% uicontrol('Style','text', 'BackgroundColor',[0.85 0.8 0.85], 'Position',[10 240 205 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[0 0], 'String','Soá liệu nhập cho điểm thứ '); % tao noi dung cua du lieu luu tru diem thu i uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[220 240 35 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String','1','tag','ttdiem'); % % ===== TAO PHAN CHU THICH CAC THONG SO DAU RA =======% uicontrol('Style','text', 'BackgroundColor',[0.9 0.8 0.9], 'Position',[470 210 210 25], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[0 0], 'String',' CÁC THÔNG SỐ ĐẦU RA '); % % ===== TAO CHU THICH GOC DETA1 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[480 180 80 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Goùc deta1'); % % ===== TAO NOI DUNG GOC DETA1 =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[570 180 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String','','tag','deta1n'); % % ===== TAO CHU THICH DO CUA GOC DETA1 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[630 180 40 22], 'HorizontalAlignment','center', 'fontname','VNITimes', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % % ===== TAO CHU THICH GOC DETA2 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[480 150 80 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Goùc deta2'); % % ===== TAO NOI DUNG GOC DETA2 =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[570 150 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String','','tag','deta2n'); % % ===== TAO CHU THICH DO CUA GOC DETA2 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[630 150 40 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % % % % ===== TAO CHU THICH GOC DETA3 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[480 120 80 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Goùc deta3'); % % ===== TAO NOI DUNG GOC DETA3 =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[570 120 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String','','tag','deta3n'); % % ===== TAO CHU THICH DO CUA GOC DETA3 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[630 120 40 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % % ===== TAO CHU THICH GOC DETA4 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[480 90 80 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Goùc deta4'); % % ===== TAO NOI DUNG GOC DETA4 =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[570 90 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String','','tag','deta4n'); % % ===== TAO CHU THICH DO CUA GOC DETA4 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[630 90 40 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % % ===== TAO CHU THICH GOC DETA5 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[480 60 80 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Goùc deta5'); % % ===== TAO NOI DUNG GOC DETA5 =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[570 60 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String','','tag','deta5n'); % % ===== TAO CHU THICH DO CUA GOC DETA5 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[630 60 40 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % % ===== TAO CHU THICH GOC DETA6 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[480 30 80 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Goùc deta6'); % % ===== TAO NOI DUNG GOC DETA6 =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[570 30 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String','','tag','deta6n'); % % ===== TAO CHU THICH DO CUA GOC DETA6 =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[630 30 40 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % ===== TAO PHAN CHU THICH CAC THONG SO DAU VAO =======% uicontrol('Style','text', 'BackgroundColor',[0.9 0.8 0.9], 'Position',[10 210 220 25], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[0.9 0.5 0], 'String',' CÁC THÔNG SỐ ĐẦU VÀO '); % ===== TAO PHAN CHU THICH THONG SO NX =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[10 180 28 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Nx'); % ===== TAO NOI DUNG NX =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[40 180 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String',' ','tag','nxn'); % ===== TAO CHU THICH DO CHO NX =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[92 180 27 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % ===== TAO PHAN CHU THICH THONG SO SX =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[122 180 28 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Sx'); % ===== TAO NOI DUNG SX =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[153 180 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String',' ','tag','sxn'); % ===== TAO CHU THICH DO CHO SX =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[205 180 27 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % ===== TAO PHAN CHU THICH THONG SO aX =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[235 180 28 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','ax'); % ===== TAO NOI DUNG aX =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[265 180 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String',' ','tag','axn'); % ===== TAO CHU THICH DO CHO aX =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[317 180 27 22], 'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % ===== TAO PHAN CHU THICH THONG SO PX =======% uicontrol('Style','text''BackgroundColor',[0.8 0.9 0.8],… 'Position',[345 180 28 22], 'HorizontalAlignment','center', 'fontname','VNITimes', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Px');% ===== TAO NOI DUNG PX =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[375 180 50 22], 'HorizontalAlignment','center', 'FontSize',12, 'ForegroundColor',[0 0], 'String',' ','tag','pxn')% ===== TAO CHU THICH DO CHO PX =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[427 180 27 22], 'HorizontalAlignment','center', 'fontname','VNITimes', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String % ===== TAO PHAN CHU THICH THONG SO Ny =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[10 150 28 22], HorizontalAlignment','center',.fontname','VNITimeFontweight','bold',.'FontSize',12, 'ForegroundColor',[1 0], 'String',' Ny');% ===== TAO NOI DUNG Ny =======% uicontrol('Style','edit','BackgroundColor',[1 1], 'Position',[40 150 50 22],'HorizontalAlignment','center''FontSize',12'ForegroundColor',[0 'String',' ','tag','nyn'); % ===== TAO CHU THICH DO CHO Ny =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8],'Position',[92 150 'HorizontalAlignment','center'fontname','VNITime'Fontweight','bold''FontSize',12, ForegroundColor',[1 1], 'String','Độ '% ===== TAO PHAN CHU THICH THONG SO Sy =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[122 150 28 22'HorizontalAlignment','center','fontname','VNITimes',.'Fontweight','bold''FontSiz'ForegroundColor',[1 0], 'String','Sy'); % ===== TAO NOI DUNG Sy =======% uicontrol('Style','edit','BackgroundColor',[1 1],'Position',[153 150 50 2'HorizontalAlignment','cente'FontSize','ForegroundColor',[0 0]'String',' ','tag','syn'); % ===== TAO CHU THICH DO CHO Ny =======% uicontrol('Style','text',.'BackgroundColor',[0.8 0.9 0.8], Position',[205 150 27 22],.'HorizontalAlignment','center',.'fontname','VNITimes'Fontweight','bol'FontSize',12'ForegroundColor',[1 1],'String','Độ % ===== TAO PHAN CHU THICH THONG SO ay =======% uicontrol('Style','text','BackgroundColor',[0.8 0.9 0.8],.'Position',[235 150 28 2'HorizontalAlignment','center', 'fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12,.'ForegroundColor',[1 0], 'String','ay'); % ===== TAO NOI DUNG ay =======% uicontrol('Style','edit', 'BackgroundColor',[1 1'Position',[265 150 50 22 'HorizontalAlignment','center',.'FontSize',12, 'ForegroundColor',[0 0],.'String',' ','tag','ayn'); % ===== TAO CHU THICH DO CHO ay =======% uicontrol('Style','text'BackgroundColor',[0.8 0.9 0.8], 'Position',[317 150 27 22]'HorizontalAlignment','center''fontname','VNI-Times', 'Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ '); % ===== TAO PHAN CHU THICH THONG SO Py =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8],'Position',[345 150 28 22],.'HorizontalAlignment','center','fontname','VNITimes','Fontweight','bold','FontSize',12, 'ForegroundColor',[1 0]'String','Py'); % ===== TAO NOI DUNG Py =======% uicontrol('Style','edit''BackgroundColor',[1 1],'Position',[375 150 50 22] 'HorizontalAlignment','center''FontSize' 'ForegroundColor',[0 0'String',' ','tag','pyn'); % ===== TAO CHU THICH DO CHO Py =======% uicontrol('Style','textBackgroundColor',[0.8 0.9 0.8] 'Position',[427 150 27 22],.'HorizontalAlignment','center''fontname','VNITimes''Fontweight','bold',.'FontSize','ForegroundColor',[1 1'String','mm') % ===== TAO PHAN CHU THICH THONG SO NZ =======% uicontrol('Style','t'BackgroundColor',[0.8 0.9 0.'Position',[10 120 28 2'HorizontalAlignment','center 'fontname','VNITim'Fontweight','bold''FontSize',1'ForegroundColor',[1 'String',' Nz'); % ===== TAO NOI DUNG Nz =======% uicontrol('Style','edit',.'BackgroundColor',[1 1],'Position',[40 120 50 22], 'HorizontalAlignment','center', FontSize',1'ForegroundColor',[0 0] 'String',' ','tag','nzn'); % ===== TAO CHU THICH DO CHO Nz =======% uicontrol('Style','te'BackgroundColor',[0.8 0.9 0'Position',[92 120 27 22], 'HorizontalAlignment','center''fontname','VNITimes',.'Fontweight','bold', 'FontSize',12,'ForegroundColor',[1 1], 'String','Độ '); % ===== TAO PHAN CHU THICH THONG SO Sz =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[122 120 28 2'HorizontalAlignment','cente…'fontname','VNI-Times','Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 0], 'String','Sz'); % ===== TAO NOI DUNG Sz =======% uicontrol('Style','edit',.BackgroundColor',[1 1],'Position',[153 120 50 22]'HorizontalAlignment','center', 'FontSize',12,.'ForegroundColor',[0 0… 'String',' ','tag','szn'); % ===== TAO CHU THICH DO CHO Nz =======% uicontrol('Style','text','BackgroundColor',[0.8 0.9 0.8],'Position',[205 120 27 22],.'HorizontalAlignment','center',.fontname','VNITimes'Fontweight','bold',.'FontSize',12, 'ForegroundColor',[1 1], 'String','Độ ') % ===== TAO PHAN CHU THICH THONG SO az =======% uicontrol('Style','text' 'BackgroundColor',[0.8 0.9 0.8], 'Position',[235 120 28 22], 'HorizontalAlignment','center', 'fontname','VNITimes', 'Fontweight','bold', FontSize',12, 'ForegroundColor',[1 0], 'String','az'); % ===== TAO NOI DUNG az =======% uicontrol('Style','edit' 'BackgroundColor',[1 1],.'Position',[265 120 50 22],.'HorizontalAlignment','center',.'FontSize',12, 'ForegroundColor',[0 0], 'String',' ','tag','azn'); % ===== TAO CHU THICH DO CHO az =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8],.'Position',[317 120 27 22],'HorizontalAlignment','center', 'fontname','VNITimes''Fontweight','bold', 'FontSize',12, 'ForegroundColor',[1 1],.'String','Độ '); % ===== TAO PHAN CHU THICH THONG SO Pz =======% uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8'Position',[345 120 28 22], 'HorizontalAlignment','center'fontname','VNI-Tim…'Fontweight','bold', 'FontSize',12,.'ForegroundColor',[1 0], 'String','Pz'); % ===== TAO NOI DUNG Pz =======% uicontrol('Style','edit', 'BackgroundColor',[1 1], 'Position',[375 120 50 22 'HorizontalAlignment','center', 'FontSize',12,.'ForegroundColor',[0 0],.'String',' ','tag','pzn'); % ===== TAO CHU THICH DO CHO Pz =======% uicontrol('Style','text',.'BackgroundColor',[0.8 0.9 0.8],.'Position',[427 120 27 22],.'HorizontalAlignment','center','fontname','VNI-Times', 'Fontweight','bol'FontSize', 'ForegroundColor',[1 1], 'String','mm') % TAO5 NUT UNDO uicontrol('Style','push',.'BackgroundColor',[1 1] 'Position',[355 270 100 25],.'HorizontalAlignment','center',.'fontname','VNITimes', 'Fontweight','bold','FontSize',12,.'ForegroundColor',[0 0], 'String','UNDO', 'callback','undo');% TAO CHU THICH VAN TOC uicontrol('Style','text', 'BackgroundColor',[0.8 0.91 0.8], 'Position',[465 270 65 22],'HorizontalAlignment','center', 'fontname','VNI-Times','Fontweight','bold',.'FontSize',1 'ForegroundColor',[1 1], 'String','Vận tốc');% TAO NOI DUNG VAN TOC uicontrol('Style','edit', BackgroundColor',[1 'Position',[532 270 65 22], 'HorizontalAlignment','center',.'FontSize',12,'ForegroundColor',[0 0], 'String','0.1','tag','vantoc');%TAO DON VI VAN TOC uicontrol('Style','text', 'BackgroundColor',[0.8 0.9 0.8], 'Position',[605 270 65 22], 'HorizontalAlignment','center','fontname','VNI-Times 'Fontweight','bold', 'FontSize',12,.'ForegroundColor',[1 1], 'String','m/phut% TAO5 NUT RESET uicontrol('Style','push', 'BackgroundColor',[1 0.9 1], Position',[355 230 100 25]… 'HorizontalAlignment','center', 'fontname','VNITimes', 'Fontweight','bold', 'FontSize',12,'ForegroundColor',[1 0.2 0],'String','RESET', 'callback','reset');% TAO NUT NHAN KET QUA uicontrol('Style','push', BackgroundColor',[1 1],'Position',[10 70 100 25]'HorizontalAlignment','center',.'fontname','VNI-Time/…'Fontweight','bold', 'FontSize',12 'ForegroundColor',[0 0'String','Nhận liệu', 'callback','inputdata'); % TAO NUT XUAT KET QUA uicontrol('Style','push', 'BackgroundColor',[1 1] 'Position',[120 70 100 25],.'HorizontalAlignment','center',.'fontname','VNI-Times', 'Fontweight','bold','FontSize',12,'ForegroundColor',[0 0], 'String','Xuất liệu','callback','exputdata');% TAO NUT MO BAN KEP uicontrol('Style','push', 'BackgroundColor',[1 1], 'Position',[230 70 105 25], HorizontalAlignment','center', 'fontname','VNITimes','Fontweight','bol'FontSize',12,.ForegroundColor',[0 0], String','Mở bàn kẹp''callback','opengrip'); % TAO NUT DONG BAN KEP uicontrol('Style','push','BackgroundColor',[1 1],.'Position',[345 70 105 25]'HorizontalAlignment','center 'fontname','VNITimes','Fontweight','bold', 'FontSize',12, 'ForegroundColor',[0 0],.'String','Đóng bàn kẹp', 'callback','closegrip');% TAO NUT HOAT DONG uicontrol('Style','push',.'BackgroundColor',[1 1]'Position',[10 40 100 25]'HorizontalAlignment','center''fontname','VNITime'Fontweight','bold', FontSize',12'ForegroundColor',[0 0],'String','START',.'callback','start'% TAO NUT HOAT DONG uicontrol('Style','push', 'BackgroundColor',[1 1],'Position',[120 40 100 25], 'HorizontalAlignment','center''fontname','VNI-Times','Fontweight','bold 'FontSize',12, 'ForegroundColor',[0 0],.'String','STOP',.'callback','STOP');% TAO NUT CONTINUE uicontrol('Style','push', 'BackgroundColor',[1 1],.'Position',[230 40 100 25],'HorizontalAlignment','center', 'fontname','VNITimes', 'Fontweight','bold','FontSize',12,.'ForegroundColor',[0 0], 'String','CONTINUE', 'callback','continue');% TAO NUT SUA DU LIEU uicontrol('Style','push', 'BackgroundColor',[1 1], 'Position',[345 40 100 25],.'HorizontalAlignment','center','fontname','VNITimes','Fontweight','bold', 'FontSize',12, 'ForegroundColor',[0 0], 'String','Sửa liệu',.'callback','modify');function ketqua()global TT a2 a3 a4formata2=150;a3=180;a4=30;%NHap cac thong so gocd1=get(findobj('tag','deta1'),'string');% lay gia tri goc deta1 (d) deta1=str2num(d1)*(pi/180); % doi sang rad d2=get(findobj('tag','deta2'),'string');% lay gia tri goc deta2 (d) deta2=str2num(d2)*(pi/180); % doi sang rad d3=get(findobj('tag','deta3'),'string');% lay gia tri goc deta3 (d) deta3=str2num(d3)*(pi/180); % doi sang rad d4=get(findobj('tag','deta4'),'string');% lay gia tri goc deta4 (d) deta4=str2num(d4)*(pi/180); % doi sang rad d5=get(findobj('tag','deta5'),'string');% lay gia tri goc deta5 (d) deta5=str2num(d5)*(pi/180); % doi sang rad d6=get(findobj('tag','deta6'),'string');% lay gia tri goc deta6 (d) deta6=str2num(d6)*(pi/180); % doi sang rad %kiem tra cac thong so goc Md=[str2num(d1) str2num(d2) str2num(d3) str2num(d4) str2num(d5) str2num(d6)]; Limitd=[-100,-25,-80,-98,0,-90]; Limitt=[100,90,80,0,180,90]; for i=1:length(Md) if ((Md(i)Limitt( s1='Giới hạn góc deta'; s2=num2str(i);s3=' Chỉ nằm khoảng từ ';s4=num2str(Limitd(i))s5=' đến '; s6=num2str(Limitt(i s7=' độ 's=[s1,s2,s3,s4,s5,s6,s7];f2=figure('Numbertitle','off', 'color',[1 1], 'menubar','none', Position',[40 290 400 90], 'name','Waning!!!', 'handlevisibility','on', 'resize','on', 'Windowstyle','modal','tag','f8'); uicontrol('Style','text', 'BackgroundColor',[1 1], 'Position',[20 360 70], 'fontname','Vni-Times', 'FontSize',14,'FontWeight','bold', 'Fontangle','italic', 'ForegroundColor','r', 'String',s);return enendC1=cos(deta1);S1=sin(deta1);C2=cos(deta2);S2=sin(deta2);C3=cos(deta3);S3=sin(d eta3);C4=cos(deta4);S4=sin(deta4);C5=cos(deta5);S5=sin(deta5);C6=cos(deta6);S6=sin(de ta6);A1=[C1,0,S1,0;S1,0,-C1,0;0,1,0,0;0,0,0,1]; A2=[C2,-S2,0,a2*C2;S2,C2,0,a2*S2;0,0,1,0;0,0,0,1]; A3=[C3,-S3,0,a3*C3;S3,C3,0,a3*S3;0,0,1,0;0,0,0,1]; A4=[C4,0,-S4,a4*C4;S4,0,C4,a4*S4;0,-1,0,0;0,0,0,1]; %A5=[C5,0,-S5,0;S5,0,C5,0;0,-1,0,0;0,0,0,1]; A5=[C5,0,S5,0;S5,0,-C5,0;0,1,0,0;0,0,0,1]; A6=[C6,-S6,0,0;S6,C6,0,0;0,0,1,0;0,0,0,1]; TE=[1,0,0,0;0,1,0,0;0,0,1,180;0,0,0,1]; TT=A1*A2*A3*A4*A5*A6*TE;for i=1:3 for j=1:3TT(i,j)=lamtron(acos(TT(i,j))*180/pi); endend set(findobj('tag','nx'),'String',num2str(TT(1,1))); set(findobj('tag','sx'),'String',num2str(TT(1,2))); set(findobj('tag','ax'),'String',num2str(TT(1,3))); set(findobj('tag','px'),'String',num2str(lamtron(TT(1,4)))); set(findobj('tag','ny'),'String',num2str(TT(2,1))); set(findobj('tag','sy'),'String',num2str(TT(2,2))); set(findobj('tag','ay'),'String',num2str(TT(2,3))); set(findobj('tag','py'),'String',num2str(lamtron(TT(2,4)))); set(findobj('tag','nz'),'String',num2str(TT(3,1))); set(findobj('tag','sz'),'String',num2str(TT(3,2))); set(findobj('tag','az'),'String',num2str(TT(3,3))); set(findobj('tag','pz'),'String',num2str(lamtron(TT(3,4)))); function ketqua2()global TT a2 a3 a4format short% ma tran gioi han cac goc Limitd=[-100,-25,-80,-98,0,90];Limitt=[100,90,80,0,180,90];TE=[1,0,0,0;0,1,0,0;0,0,1,180;0,0,0,1]; %ma tran bo goc hien hanhGHH=[0 0 -90 90 0];%NHap cac thong so gocnxd=get(findobj('tag','nxn'),'string');nx=str2num(nxd)*pi/180; sxd=get(findobj('tag','sxn'),'string');sx=str2num(sxd)*pi/180;xd=get(findobj('tag','axn'),'strin g');ax=str2num(axd)*pi/180;pxd=get(findobj('tag','pxn'),'string');px=str2num(pxd); yd=get(findobj('tag','nyn'),'string');ny=str2num(nyd)*pi/180;syd=get(findobj('tag','syn'),'stri ng');y=str2num(syd)*pi/180;ayd=get(findobj('tag','ayn'),'string');ay=str2num(ayd)*pi/180;p yd=get(findobj('tag','pyn'),'string');py=str2num(pyd);zd=get(findobj('tag','nzn'),'string'); nz=str2num(nzd)*pi/180;szd=get(findobj('tag','szn'),'string');sz=str2num(szd)*pi/180; azd=get(findobj('tag','azn'),'string');az=str2num(azd)*pi/180;pzd=get(findobj('tag','pzn'),'stri ng');pz=str2num(pzd);R=[nx,sx,ax;ny,sy,ay;nz,sz,az];for i=1:3 for j=1:3 R(i,j)=cos(R(i,j)); endendK=eye(4,4);K(1,4)=px;K(2,4)=py;K(3,4)=pz;for i=1:3 for j=1:3K(i,j)=R(i,j); endendK=K*inv(TE);nx=K(1,1);sx=K(1,2);ax=K(1,3);ny=K(2,1);sy=K(2,2);ay=K(2,3); nz=K(3,1);sz=K(3,2);az=K(3,3);px=K(1,4);py=K(2,4);pz=K(3,4);%TINH TOAN CAC GOC DO% tinh goc deta1deta1=atan2(py,px);d1=lamtron(deta1*180/pi);% if (d1Limitt(1))% d1=d1+180% endd1t=deta1; % GOC DA TINH DUNG%tinhgocdeta234234=lamtron(az);C234=lamtron(cos(d1t)*ax+sin(d1t)*ay);deta234 =lamtron(atan2(S234,C234));d234=deta234*180/pi;% if(d234(Limitt(2)+Limitt(3)+Limitt(4))) % d234=d234+180% endd234t=deta234; % GOC DA TINH DUNG% %tinh goc deta3(phuong an 1)k1=(cos(d1t)*px+sin(d1t)*py)-a4*cos(d234t);k2=pz-a4*sin(d234t); C3=lamtron((k1^2+k2^2-a2^2-a3^2)/(2*a2*a3));S31=lamtron(-(1-C3^2)^(1/2)); S32=lamtron((1-C3^2)^(1/2));S3=S31;deta3=lamtron(atan2(S3,C3));d3=deta3*180/pi; % if (d3Limitt(3))% d3=d3+180;% endd3t=d3*pi/180; % GOC DA TINH DUNG%tinh goc deta 2(phuong an 1)S2=lamtron(k2*(a3*cos(d3t)+a2)sin(d3t)*a3*k1)/((cos(d3t)*a3+a2)^2+(sin(d3t)*a3)^2);C2=(k1*(a3*cos(d3t)+a2)+k2*sin(d 3t)*a3)/((cos(d3t)*a3+a2)^2+(sin(d3t)*a3)^2);deta2=lamtron(atan2(S2,C2));d2=deta2*180 /pi;% if (d2Limitt(2))% d2=d2+180;% endd2t=d2*pi/180; %GOC DA TINH DUNG% tinh goc deta4 (phuong an 2)d4=d234-d3d2;%if(d4Limitt(4))% d4=d4+180;% endd4t=d4*pi/180; % GOC DA TINH DUNG%tinh goc deta5S5=cos(d234t)*(cos(d1t)*ax+sin(d1t)*ay)+sin(d234t)*az; C5=sin(d1t)*ax-cos(d1t)*ay;deta5=lamtron(atan2(S5,C5));d5=deta5*180/pi;if (d5Limitt(5))d5=d5+180;endd5t=d5*pi/180;% tinh goc deta6 S6=lamtron(-sx*(cos(d5t)*cos(d234t)*cos(d1t)-sin(d5t)*sin(d1t))+ sy*(cos(d5t)*cos(d234t)*sin(d1t)+sin(d5t)*cos(d1t))+cos(d5t)*sin(d234t)*sz); S6p=lamtron( -cos(d5t)*( cos(d234t)*(cos(d1t)*sx+sin(d1t)*sy)+sin(d234t)*sz )sin(d5t)*(sin(d1t)*sxcos(d1t)*sy));C6=lamtron(sin(d234t)*(cos(d1t)*sx+sin(d1t)*sy)+cos(d234t)*sz);eta6=lamtr on(atan2(S6p,C6));6=deta6*180/pi;if (d6Limitt(6)) d6=d6+180;endd6t=d6*pi/180;D=[d1,d2,d3,d4,d5,d6];for i=1:length(D) if abs(D(i))