4. Thiết kế phần mềm điều khiển Robot song song
4.3.Chƣơng trình hoạt động của CPU-MAIN
CPU-MAIN thực hiện trao đổi dữ liệu với các CPU-DRIVER của các khớp (hình 4.4).
Sau khi khởi động, CPU-Main sẽ xác lập lại các CPU-DRIVER của từng khớp, dị tìm điểm chuẩn Zero và quay các khớp về vị trí khởi động.
Trong trƣờng hợp điều khiển trực tiếp khơng sử dụng chƣơng trình từ máy tính, CPU-Main sẽ vận hành chƣơng trình OB1a điều khiển robot.
Trong chế độ điều khiển từ máy tính, CPU-Main sẽ trao đổi dữ liệu với CPU-PC để nhận dữ liệu từ máy tính. Chƣơng trình CPU-Main cho phép nhận giá trị kiểu hoạt động, lựa chọn và gửi các tham số, giá trị điều khiển hoặc giá trị vị trí tới CPU-DRIVER của từng khớp. Tần số lấy mẫu là 10 ms.
Giải thuật chƣơng trình điều khiển trực tiếp OB1a trình bày trên hình 4.5. Khi chƣơng trình OB1a đƣợc tác động, CPU-Main sẽ tiến hành đọc mã thực hiện lệnh mới ở dạng mã máy trung gian, sau đĩ tiến hành lựa chọn mã lệnh và thực hiện lệnh:
Lệnh “goto”: di chuyển con trỏ lệnh đến vị trí mới Lệnh “move”: điều khiển robot đến vị trí mới
Lệnh “if … then goto”; điều khiển con trỏ lệnh theo giá trị lối vào …
Đồng thời CPU-Main lập cờ chờ đợi hồn thành lệnh. Trƣờng hợp khơng cĩ lệnh mới, chuỗi lệnh trên khơng thực hiện. Sau khi đã hồn thành lệnh, sẽ lập cờ để thực hiện lệnh mới.
Trên hình 4.6 trình bày giản đồ thuật tốn xử lý và điều khiển từ chƣơng trình chính của CPU-Main. Thuật tốn này cho phép lựa chọn thuật tốn điều khiển kiểu PID, PIV trên bộ vi xử lý CPU-Main.
47
Hình 4.4. Giản đồ chương trình chính hoạt động của CPU-Main
Start Từ ĐK của Frame nhận Chế độ ĐK Timer =0,1,2,3 ? từ PC ? 10 ms ?
Quay các khớp tới vị trí khởi động
Trao đổi dữ liệu với từng Drive-CPU của khớp Trao đổi dữ liệu với CPU-PC qua chuẩn SPI
Reset các Driver-CPU của các khớp
Tạo kết nối với CPU-PC truyền nối tiếp với máy tính
Xử lý chương trình
Thuật toán OB1a để xác định
ĐK khớp hoạt động của tay máy Dò tìm điểm chuẩn zero của các khớp
Nhận giá trị
kiểu hoạt động Gửi giá trị tham sốtới từng Driver-CPU tới từng Driver-CPUGửi giá trị ĐK tới từng Driver-CPUGửi giá trị vị trí
=0 =1 =2 =3
NY Y
48
Hình 4.5. Giải thuật chương trình vận hành robot OB1a.
OB1a Lựa chọn mã lệnh Đã hoàn thành lệnh Thực hiện lệnh mới =0,1,2,3,... ? ? ? Return Return N Y
Đọc mã lệnh mới ở dạng mã máy trung gian
Lệnh “goto...” Lệnh “move...” Lệnh “if... then goto...”
Lập cờ chờ đợi hoàn thành lệnh
Lập cờ thực hiện lệnh mới
Di chuyển Điều khiển Điều khiển (adsbygoogle = window.adsbygoogle || []).push({});
con trỏ lệnh đến tay máy đến con trỏ lệnh theo giá trị
vị trí mới vị trí mới lối vào
=0 =1 =2 =...
YN N
49
Hình 4.6. Giản đồ thuật tốn xử lý và điều khiển.
Return
Thuật toán xử lý & Đ.khiển
Lựa chọn thuật toán =0,1?
Thuật toán PID Thuật toán PIV
50