Mô hình sẽ được điều khiển hoạt động theo các chế độ khác nhau. Khi bắt đầu, để việc tính toán và điều khiển được dễ dàng thì robot sẽ được di chuyển đến chế độ vị trí ban đầu (set home). Việc thiết lập chế độ ban đầu này đóng vai trò đặc biệt quan trọng trong quá trình điều khiển robot chạy theo quỹ đạo mong muốn. Chế độ ban đầu có thể được hiểu là đưa robot đến vị trí gốc định trước để từ đó việc tính toán góc, tọa độ… điều khiển robot hoạt động một cách chính xác. Sau đó phần hoạt động của robot sẽ có 2 chế độ lựa chọn là chạy theo quỹ đạo đường thẳng, hình chữ nhật, hình tam giác, hình tròn cho trước (Task 2) và chạy theo quỹ đạo bất kỳ (Task 3). Khi robot hoạt động sẽ hiển thị các thông số cũng như quỹ đạo đi lên màn hình hiển thi (Task 4).
Ở chế độ vị trí ban đầu, đặt giá trị 2 góc của động cơ mong muốn mà ta chọn lần lượt là 104, 76. Sau đó, ta cho góc hiện tại của vị trí robot đang nhận bằng 0 và tính được số xung trên một độ là 25600/360. Nếu góc hiện tại của robot mà bằng góc mong muốn thì thỏa mãn chế độ. Nếu góc hiện tại lớn hơn góc mong muốn thì ta lấy n bằng độ chênh lệch giữ góc hiện tại và góc mong muốn, cho chân dir (đổi chiều) lên giá trị 1. Nếu góc mong muốn bằng 0 thì n bằng góc hiện tại sau đó tính được số bước chính xác để quay ra độ mong muốn và điều khiển động cơ chạy. Nếu góc hiện tại nhỏ hơn góc mong muốn thì ta lấy n bằng độ chênh lệch giữ góc mong muốn và góc hiện tại, cho chân dir (đổi chiều) vẫn giá trị 0, tính được số bước chính xác để quay ra độ mong muốn và điều khiển động cơ chạy.
Ở chế độ chạy theo quỹ đạo cho trước, đầu tiên ta cũng điều khiển robot di chuyển đến vị trí ban đầu có góc quay của 2 động cơ lần lượt là 104, 76. Sau đó kích chọn nút defaut (defaut = 1), động cơ quay, di chuyển đến tọa độ có X = 60, Y = 124. Tiếp theo kích nút nhấn chọn chế độ hoạt động theo quỹ đạo mặc định (Type 1 == 1). Chế độ được kích hoạt hoạt động. Ở đây ta có thể tùy ý chọn chế độ hoạt động theo từng quỹ đạo. Chọn các chế độ hoạt động theo quỹ đạo đường thẳng, hình chữ nhật, hình tam giác, hình tròn tương ứng với các nút nhấn B1 (B1=1), B2 (B2=1), B3 (B3=1), B4 (B4=1). Robot sẽ hoạt động theo quỹ đạo được chọn.
Hình 3-7: Lưu đồ giải thuật của chế độ chạy theo quỹ đạo bất kỳ
Ở chế độ chạy theo quỹ đạo bất kỳ, đầu tiên ta cũng cho robot di chuyển đến vị trí ban đầu có góc quay của 2 động cơ lần lượt là 104, 76. Nhấn nút nhấn của chế độ chạy quỹ đạo bất kỳ (Type 2 ==1). Sau đó hệ thống sẽ tiến hành thuật toán tự tìm quỹ đạo đi cho robot. Nhận tọa độ điểm muốn đến có tọa độ x, y (C = [Px,Py]), tọa độ của 2 động cơ lần lượt là 70;0 , 70;0. Tiếp theo thực hiện tính khoảng cách của điểm C so với 2 tọa độ động cơ và tính độ chênh lệch chiều dài các thanh
Sau khi tính xong khoảng cách tọa độ điểm cần đến, chọn chế độ hoạt động (Mode 1 == 1, hoặc Mode 2 == 1, hoặc Mode 3 == 1, hoặc Mode 4 == 1) tương ứng với từng không gian làm việc khác nhau của robot. Có 4 không gian làm việc khác nhau tương ứng của robot đó là Mode ++ (a=1, b=1), Mode +- (a=1, b=-1), Mode -+ (a=-1, b=1), Mode -- (a=-1, b=-1). Khi ta chọn được một chế độ làm việc, hệ thống sẽ bắt đầu tiến hành so sánh các điều kiện 1 C K , 2 C
K với l, lvà kiểm tra
1 C K , 2 C K (KC 0hoặc 0 C
K ), sau đó dựa vào các điều kiện so sánh và kiểm tra này sẽ thực hiện tương ứng các bước tính toán giá trị theo phương pháp SCA để cho ra kết quả
1
,A1,val1,2,A2,val2. Tính giá trịCA1,CA2(CA1 C A1,CA2 C A2). Tiếp theo tính giá trị C( C CA1 1 CA2 2 CA2 1 CA1 2 ). So sánh điều kiện Cđể tìm ra chế độ lắp ráp làm việc của robot. Nếu C 0thì robot có 2 chế độ lắp ráp +1 (làm việc vùng trên), -1 (làm việc vùng dưới). Ở đề tài này, nhóm chỉ tập trung điều khiển robot hoạt động ở vùng làm việc trên.
Hình 3-8: Lưu đồ giải thuật của chế độ các nút nhấn
Ở chế độ chạy và hiển thị theo các nút nhấn, đầu tiên ta cũng điều khiển robot di chuyển đến vị trí ban đầu có góc quay của 2 động cơ lần lượt là 104, 76. Sau đó thực hiện kiểm tra nút default (B_default) và các nút nhấn chọn hiển thị (B_Theo, B_Real). Nếu kích nút nhấn default (B_default ==1) thì thực hiện điều khiển robot chạy đến vị trí có X = 60, Y = 124 tương ứng với 2 góc động cơ có giá trị là 69,7144 và 43,0769 (tìm ra giá trị của góc động cơ bằng cách nhập tọa độ của X, Y tương ứng vào phương trình động học nghịch) tương tự như theo giải thuật của chế độ thiết lập về vị trí ban đầu. Đồng thời kiểm tra điều kiện của các nút hiển thị. Nếu nút Theory được nhấn
(B_Theo ==1) thì sẽ hiển thị quỹ đạo làm việc theo lý thuyết của robot còn nếu không được nhấn thì sẽ hiển thị quỹ đạo thực tế của robot.
CHƯƠNG 4: THI CÔNG, THỰC NGHIỆM, TỔNG HỢP VÀ PHÂN TÍCH