4. Thiết kế phần mềm điều khiển Robot song song
4.4.Chƣơng trình hoạt động của CPU-DRIVER cho các khớp
CPU-DRIVER thực hiện điều khiển các khớp theo chƣơng trình từ CPU-Main hoặc từ máy tính. Chu kỳ lấy mẫu là 10 ms (hình 4.7).
Trong trƣờng hợp điều khiển từ CPU-Main, kiểu điều khiển là PID hoặc PIV, đƣợc thực hiện bằng chính chƣơng trình chứa trong từng CPU-DRIVER cho mỗi khớp.
Trƣờng hợp điều khiển từ máy tính, CPU-DRIVER chỉ nhận kết quả điều khiển đã đƣợc xử lý trong máy tính.
Các tín hiệu quay từ Encoder của khớp sẽ đƣợc CPU-DRIVER đo và xử lý. Khi cĩ Interupt từ Serial Bus (hình 4.8), chƣơng trình cho phép đặt lại các tham số PID, PIV, nhận giá trị vị trí mới hoặc nhận điều khiển trực tiếp từ máy tính.
Hình 4.7. Giản đồ chương trình chính hoạt động của CPU-DRIVER
Start Kiểu ĐK Timer =0,1,2 ? 10 ms ? Xuất giá trị ĐK
Thuật toán PID Thuật toán PIV Xuất giá trị ĐK trực tiếp từ PC (Uđk)
51
Hình 4.8. Giản đồ chương trình ngắt từ CPU-Main
4.5. Kết luận
Trong phần Chƣơng trình điều khiển robot đã thiết kế các phần chính của chƣơng trình điều khiển robot:
- Chƣơng trình máy tính điều khiển robot.
- Chƣơng trình hoạt động của CPU-MAIN điều hành trung tâm và kết nối máy tính.
- Chƣơng trình hoạt động của CPU-DRIVER cho các khớp.
Hệ thống chƣơng trình cho phép điều khiển robot từ máy tính hoặc trực tiếp từ bộ vi xử lý chính trong hệ điều khiển.
Xác định từ ĐK của Frame truyền =0,1,2,3 ?
Return
Đặt lại tham số PID Đặt lại tham số PIV Nhận giá trị vị trí mới Nhận giá trị ĐK trực tiếp từ PC
=0 =1 =2 =3
Interupt từ
Trả lại giá trị Count Nhận giá trị từ Serial Bus
52