h. Hình ảnh của module ñ iều khiển
3.2.2.3. Lập trình Module ñ iều khiển ñộ ng cơ secvo theo tọa ñộ
Sử lý ñiều khiển ñộng cơ DC theo tọa ñộ, cần thiết bị Encoder ñể kiểm soát vòng quay của ñộng cơ.
Với ñặc trưng của ñiều khiển ñộng cơ là cần ñiều khiển 2 thông số là
ñiều khiển tốc ñộ và tọa ñộ. ðộng cơ DC có thiết bị kiểm soát encoder cũng làm việc bằng xung, nên ta cũng cần phải phải thực hiện 2 công việc:
• ðiều khiển ñộng cơ theo tọa ñộ xung
• Quy ñổi tọa ñộ xung sang tọa ñộ làm việc.
Trong ñề tài này vì không ñủ thời gian nên tôi không thực hiện ñiều khiển tốc ñộ ñộng cơ chất lượng cao, phần ñiều khiển tốc ñộ chỉñược ñặt bởi một giá trịñộ rộng xung cốñịnh ñểñiều khiển tốc ñộñộng cơ một cách tương
ñối.
• ðiều khiển ñộng cơ theo tọa ñộ xung
ðể ñiều khiển ñộng cơ theo tọa ñộ xung cần phải sử lý 2 thao tác là: (1)Xử lý tín hiệu của encoder ñể nhận ñược tọa ñộ xung của ñộng cơ và (2)
ðiều khiển ñộng cơ theo tọa ñộ xung.
o Sử lý tín hiệu encoder:
Encoder là thiết bị ñể ñếm vòng quay và kiểm soát góc của ñộng cơ, không phải cứ lắp encoder vào là chúng ta lấy ñược tọa ñộ và góc của ñộng cơ vì: Encoder ñưa tín hiệu chỉ dưới dạng xung với 3 kênh A,B,Z. Biết ñược là kênh A lệch pha với kênh B là 900 và quay ñược một vòng thì z sẽ phát 1 xung.
Vậy muốn lấy ñược tọa ñộ và tốc ñộ nhờ encoder ta cần phải sử lý bằng lập trình mới có thể lấy ñược tín hiệu. Ở ñây tôi tính toán và lập trình cho module encoder 200xung/vòng ñể sử dụng trong mô hình.
Mục tiêu của việc lập trình ñể nhận tín hiệu encoder là phải nhận biết
ñược tốc ñộ và tọa ñộ thông qua các xung mà encoder ñưa về. Phân tích tín hiệu hai kênh A,B [2], [3]
Bảng 3.3. trạng thái tín hiệu ở 2 kênh A,B
A 1 0 0 1 1 0 0 1 1
B 0 0 1 1 0 0 1 1 0
ðề tài sử dụng một kênh ngắt ngoài ñể nhận biết từng xung của encoder ñưa về. Lợi thế sử dụng ngắt ngoài là ñộưu tiên chương trình rất cao, việc bắt trượt xung rất thấp.
Encoder 200 xung/vòng, ñộng cơ tối ña là 2700 vòng/phút, tần số mỗi xung là fxung = 2700*200/60=9000 xung/s, vậy 1 xung ≈ 111µs. Thạch anh 11,0592Mhz, thời gian sử lý một lệnh của chip là 0.09µs sẽ thực hiện một lệnh. Vậy thời gian sử lý một lệnh nhanh hơn 1233 lần một lần ngắt ñếm xung encoder, tần số của xung encoder gần như không ảnh hưởng ñến quá trình sử
lý của hệ thống.
Nhiệm vụ của lập trình nhận biết tín hiệu encoder phải nhận biết ñược
ñộng cơ quay thuận – quay ngược, nhận biết tọa ñộ của ñộng cơ.
Quan sát tín hiệu ở 2 kênh A,B ở trên bảng trên, xét sườn xuống của tín hiệu A và ñối chiếu xuống tín hiệu B thì: ðộng cơ quay thuận B mang giá trị 1, ñộng cơ quay ngược B mang giá trị 0. Nhờ ñặc ñiểm này ta có thể biết
ñược chiều của ñộng cơ. Như vậy việc lập trình nhận biết tọa ñộ với encoder
Hình 3.30: Giải thuật nhận biết tọa ñộ
o ðiều khiển ñộng cơ theo tọa ñộ
ðể ñiều khiển ñược tọa ñộ ñộng cơ, cần phải kết hợp với module sử
lý tín hiệu encoder ñể biết ñược tọa ñộ hiện tại của thân robot. Sử dụng lợi thế
sẵn có của phần cơ khí ñược thiết kế có cho phép kẹp ñịnh tâm tại tất cả các cơ cấu của máy CNC cũng như robot. Chính vì vậy không cần yêu cầu quá cao về tọa ñộ thân robot sử dụng ñộng cơ DC-secvol. Sai số cho phép của phần thiết kế cơ khí là: ±0.5mm nên việc lập trình ñiều khiển tọa ñộ thân robot cũng không ñòi hỏi ñộ chính xác quá cao. Mặt khác vì thời gian làm ñồ
án có hạn nên tôi cũng không thểñi sâu vào khâu này.
Như vậy thuật toán ñiều khiển tọa ñộ ñộng cơ bước cũng khá ñơn giản. Lưu ñồ ñược lập trình như hình 3.31:
Lưu ñồ thuật toán hình 3.31 cho phép robot tự hiệu trỉnh lại vị trí khi chạy quá vị trí do quán tính.
Lưu ñồ thuật toán trên ñã thực hiện thành công trên mô hình Cánh tay robot.