sát trên Controldesk.
a. Trình tự thực hiện mô phỏng
Hình 3.13. Lưu đồ thuật toán thực hiện ứng dụng.
b. Chương trình mô phỏng hoạt động cho các khớp.
Việc điều khiển hoạt động của Robot dựa vào việc điều khiển tuần tự các khớp của Robot. Nhóm đã đưa ra một phương thức điều khiển cho Robot là điều khiển theo thứ tự từ khớp 1 cho tới khớp 6 (tay gắp). Tức là khớp hoạt động đầu tiên sẽ là khớp 1 sau đó
Khởi động Matlab
Xây dựng mô hình Simulink
Thiết lập các tham số mô phỏng Ctrl+ E Tạo mã DSP bằng Ctrl + B Stop Tạo layout: File/New/Layout Gán Biến từ Simulink vào Controldesk Chạy ứng dụng Khởi động ControlDesk
Tạo đường dẫn tới thư mục vừa tạo
Chương 3. Thiết kế bộ điều khiển cho Robot
52
lần lượt tới khớp 2, khớp 3 và cuối cùng là tới tay gắp. Robot sẽ thực hiện việc di chuyển tay gắp từ vị trí A qua vị trí B và trở về A sau khi hoàn thành nhiệm vụ ở B. Khớp sau chỉ hoạt động khi các khớp trước nó đã hoàn thành được nhiệm vụ. Trình tự này có thể thay đổi tùy vào việc sắp xếp thứ tự các khối mô phỏng khác nhau.
Bảng 3.4. Giá trị góc và khoảng mở các khớp tại hai điểm A và B
Khớp 1 2 3 4 5 6 Giá trị góc Khoảng mở tay gắp Tại A 60o 50o 60o 120o 190o 20mm Giá trị góc Khoảng mở tay gắp Tại B 90o 80o 100o 50o 110o 55mm
Dựa vào bảng số liệu thực nghiệm thu được, ta tiến hành xây dựng thuật toán điều khiển cánh tay Robot từ vị trí A tới vị trí B và tiến hành mô phỏng trên matlab.
Chương trình mô phỏng trên Matlab
Khớp 1:
Chương 3. Thiết kế bộ điều khiển cho Robot
53
Khớp 1 được điều khiển nhờ vào tín hiệu phản hồi thu được từ cổng ADC5 trên dSPACE 1104. Tín hiệu phản hồi về được nhân với 10 vì khi đưa điện áp vào dSPACE thì điện áp đã bị giảm đi 10 lần.
Muốn khớp 1 và các khớp khác quay từ vị trí A sang B cần nhập giá trị góc mở của khớp 1 tại vị trí A và vị trí B. Giá trị các góc của các khớp tại các vị trí A và B có thể thu lại được bằng cách sử dụng bộ điều khiển bằng tay điều khiển Robot tới vị trí cần đạt sau đó ghi lại số liệu các góc của các khớp hiển thị trên màn hình ControlDesk.
Đầu In1 là đầu để xác định xem Robot sẽ quay từ A về B hay từ B trở về A. Khi In =1 thì Robot đi từ A về B. khi In=0 thì Robot đi từ B về A.
Khối K11 và khối K12 đặt giá trị bằng hệ số tỉ lệ giữa góc quay và độ lệch điện áp của khớp 1. K11=K12=61,308. Khối Gain3 đặt giá trị bằng 1/61,308
Do kết cấu cơ khí không thật sự chính xác, đồng thời tránh sai số trong quá trình vận hành nên nhóm đã chọn độ sai lệch cho mỗi góc quay là 5º. Giá trị góc này được quy đổi thành giá trị điện áp phản hồi tương ứng của từng khớp. Giá trị này tương ứng với khớp 1 là: 0,081556 V
Giá trị thu được sẽ được so sánh nhờ khối : Relational Operator trong Matlab
Nếu giá trị sai lệch lớn hơn giá trị sai lệch trên, tín hiệu logic sẽ được nhân với 0,5 để chuyển kiểu dữ liệu sang double đưa ra cổng DAC 1, biến thành điện áp 5V để điều khiển việc quay sang trái của piston.(hình 3.16). Ngược lại nếu giá trị sai lệch nhỏ hơn giá trị sai lệch dưới thì tín hiệu logic sẽ được nhân với 0,5 đưa ra DAC 2, biến thành điện áp để điều khiển việc quay sang phải của piston (hình 3.15).
Khớp 1 hoàn thành nhiệm vụ khi giá trị sai lệch trả về nằm trong khoảng cho phép hay góc quay thực tế và góc quay đặt có chênh lệch trong khoảng từ -5° tới 5° tương đương với khoảng lệch so sánh là : 0,08155 V. Khi chưa hoàn thành nhiệm vụ thì giá trị logic qua các cổng logic OR vẫn bằng 1. Chính tín hiệu này sẽ làm cho các khớp sau không hoạt động khi khớp 1 vẫn đang hoạt động. Khi khớp 1 hoàn thành nhiệm vụ, đạt được góc có giá trị trong khoảng mong muốn thì tín hiệu đầu ra logic OR sẽ bằng 0. Tín hiệu này sẽ là tín hiệu cho phép khớp 2 hoạt động.
Chương 3. Thiết kế bộ điều khiển cho Robot
54
Hình 3.15. Kết quả mô phỏng khớp 1 khi góc quay thực tế nhỏ hơn so với góc cần đạt.
Chương 3. Thiết kế bộ điều khiển cho Robot
55
Khớp 2:
Hình 3.17. Chương trình mô phỏng điều khiển khớp 2.
Khớp 2 được điều khiển nhờ tín hiệu phản hồi từ chiết áp vít xoay qua cổng ADC6 Khớp 2 hoạt động tương tự như khớp 1, chỉ khác là khớp 2 chỉ hoạt động khi có tín hiệu điều khiển từ khớp 1 đưa tới. Khi khớp 1 hoạt động, đầu vào In2 có giá trị logic là 1, qua khối Product và khối Sum thì giá trị sai lệch mang đi so sánh bằng 0. Giá trị 0 luôn nằm trong khoảng sai lệch cho phép nên khớp 2 sẽ không hoạt động. Khớp 2 so sánh sai lệch tương tự như khớp 1 nhưng đầu đưa điện áp điều khiển ra là cổng DAC 3 và DAC 4.
Giá trị nhập vào các khối: - K21=K22= -67,689.
- Khối Gain3 đặt giá trị bằng -1/67,689
Do khớp 2 cũng là khớp quay nên giá trị sai số cũng đặt bằng điện áp tương đương với giá trị góc 5º và bằng 0,073867 V.
Khi khớp 2 hoàn thành nhiệm vụ, tín hiệu logic qua khối so sánh qua cổng OR, kết hợp với tín hiệu logic từ cổng OR từ khớp 1 đưa xuống làm tín hiệu điều khiển cho phép hoạt động của khớp 3.
Chương 3. Thiết kế bộ điều khiển cho Robot
56
Hình 3.17. Kết quả mô phỏng khớp 2 hoạt động khi góc thực tế nhỏ hơn góc cần đạt.
Chương 3. Thiết kế bộ điều khiển cho Robot
57
Khớp 3:
Hình 3.19. Chương trình mô phỏng điều khiển khớp 3.
Khớp 3 được điều khiển nhờ tín hiệu phản hồi từ chiết áp vít xoay qua cổng ADC7 Khớp 3 hoạt động tương tự như khớp 2. Đầu ra điện áp điều khiển được đưa ra qua cổng DAC5 và DAC6 của card dSPACE 1104. Tín hiệu logic sau khi so sánh và tín hiệu điều khiển từ khớp 2 xuống khớp 3 sẽ là tín hiệu điều khiển cho phép khớp 4 hoạt động. Khớp 4 sẽ chỉ hoạt động được khi các khớp 1,2 và 3 đã thực hiện xong nhiệm vụ của mình.
Các giá trị nhập vào các khối matlab: - K31=K32=58,388.
- Khối Gain3 đặt giá trị bằng 1/58,388
Do khớp 3 cũng là khớp quay nên giá trị sai số cũng đặt bằng điện áp tương đương với giá trị góc 5º và bằng 0,085633 V
Chương 3. Thiết kế bộ điều khiển cho Robot
58
Hình 3.20. Kết quả mô phỏng khi khớp 3 hoạt động khi góc thực tế nhỏ hơn góc cần đạt.
Chương 3. Thiết kế bộ điều khiển cho Robot
59
Khớp 4:
Hình 3.22. Chương trình mô phỏng điều khiển khớp 4.
Khớp 4 được điều khiển nhờ tín hiệu phản hồi từ chiết áp vít xoay qua cổng ADC8 Khớp 4 hoạt động tương tự như với khớp 3. Đầu ra điện áp điều khiển các van hoạt động sang trái hay sang phải được đưa qua cổng DAC 7 và DAC8
Do Dspace 1104 chỉ có 4 cổng ADC và 8 cổng DAC trực tiếp trong khi để điều khiển được Robot cần dùng tới 6 cồng ADC để đọc tín hiệu điện áp phản hồi vào và 12 cổng để đưa điện áp điều khiển ra. Do thiếu 2 đầu vào trực tiếp nên nhóm đã sử dụng khối Mux để đo đồng thời tín hiệu ADC từ cổng ADC1 và ADC 2 sau đó dùng khối DEMUX để tách tín hiệu từ 2 khối này đưa vào khớp 5 và khớp 6. Đầu ra còn thiếu 4 chân nên nhóm đã sử dụng thêm các chân I/O từ cổng Digital I/O làm chân đưa điện áp ra ngoài. Điện áp đầu ra của các chân I/O này khi có tác động mức cao vào là 5V.
Giá trị các khối được đặt như sau: - K41=K42=63,102.
- Khối Gain1 đặt giá trị bằng 1/63,102
Do khớp 3 cũng là khớp quay nên giá trị sai số cũng đặt bằng điện áp tương đương với giá trị góc 5º và bằng 0,07924 V
Tín hiệu logic sau khi khớp 4 và các khớp dưới nó hoàn thành nhiệm vụ sẽ là tín hiệu điều khiển cho khớp 5 hoạt động.
Chương 3. Thiết kế bộ điều khiển cho Robot
60
Hình 3.23. Kết quả mô phỏng khớp 4 hoạt động khi góc thực tế lớn hơn góc cần đạt.
Chương 3. Thiết kế bộ điều khiển cho Robot
61
Khớp 5:
Hình 3.25. Chương trình mô phỏng điều khiển cho khớp 5.
Khớp 5 hoạt động tương tự các khớp trên nó. Đầu đưa điện áp phản hồi về từ chiết áp vít xoay khớp 5 được đưa qua cổng ADC2 qua khối MUX ADC và DEMUX để tách tín hiệu phản hồi ra.
Khớp 5 có các thông số sau: - K51=K52=58,388.
- Khối Gain3 đặt giá trị bằng 1/58,388 - Độ sai lệch là 0,085648 V
Tín hiệu logic sau khi khớp 5 và các khớp dưới nó hoàn thành nhiệm vụ sẽ là tín hiệu điều khiển cho khớp 6 hoạt động.
Chương 3. Thiết kế bộ điều khiển cho Robot
62
Hình 3.26. Kết quả mô phỏng khớp 5 hoạt động khi góc thực tế lớn hơn góc cần đạt.
Chương 3. Thiết kế bộ điều khiển cho Robot
63
Khớp 6:
Hình 3.28. Chương trình mô phỏng điều khiển cho khớp 6 ( tay gắp).
Khớp 6 là khớp đặc biệt, giá trị đặt vào không phải giá trị góc mở mà lại là giá trị khoảng mở của tay gắp. Tuy vậy việc tính toán sai lệch với tín hiệu thực tế đưa về cũng tương tự như các khớp trên.
Nhập vào khối mô phỏng các giá trị: - K61=K62=-17,510.
- Khối Gain3 đặt giá trị bằng -1/17,510.
Qua quá trình thực nghiệm em đã chọn giá trị sai lệch cho phép của khớp 6 trong khoảng từ -10mm đến 10mm. Giá trị sai lệch này được quy đổi ra mức điện áp tương ứng của khớp 6 là: 0,57109 V
Tín hiệu logic sau khi khớp 6 hoàn thành nhiệm vụ sẽ được xuất ra đèn báo. Đèn báo sẽ chuyển sang màu xanh khi tất cả các khớp đã tới được vị trí B đặt trước.
Chương 3. Thiết kế bộ điều khiển cho Robot
64
Hình 3.29. Kết quả mô phỏng khớp 6 khi khoảng mở thực tế nhỏ hơn giá trị đặt.
Chương 3. Thiết kế bộ điều khiển cho Robot
65
Tổng hợp:
Hình 3.30. Chương trình tổng quát điều khiển Robot.
Sau khi các khớp hoàn thành nhiệm vụ đầu tiên từ A đến B, để đưa các khớp đi ngược lại quá trình vừa thực hiện tức là từ B về A ta thực hiện việc chuyển đổi các giá trị góc đặt cho trước. Điều này sẽ giúp các khớp hoạt động ngược lại quá trình ban đầu và bắt đầu từ khớp 1 tới khớp 2,3,4,5 và khớp 6. Để đảo chiều chuyển động này ta sử dụng một nút điều khiển trên Control Desk. Nút này sẽ liên kết với khối Manual Switch trong hình trên làm thay đổi giá trị của khối này lên 1 hoặc xuống 0. Các tín hiệu này sẽ được đưa vào các đầu In 1 của các khối khớp.
Quá trình đi từ B trở về A tương tự như quá trình đi từ A tới B. Người dùng có thể can thiệp vào bất kì thời điểm nào thông qua 2 nút nhấn trên màn hình ControlDesk
Kết quả:
Quá trình mô phỏng đã cho ra kết quả như tính toán. Tuy nhiên trong quá trình chạy thực nghiệm giá trị thực tế trả về chịu nhiều ảnh hưởng của các tác nhân môi trường và đặc biệt từ cơ cấu cơ khí của Robot. Chính sự sai lệch này đã ảnh hưởng rất nhiều tới quá trình hoạt động của Robot.