CHƯƠNG 5 : MÔ PHỎNG ĐỘNG HỌC VÀ GIẢI THUẬT ĐIỀU KHIỂN
5.2 Thiết kế giải thuật điều khiển
Phần này trình bày lưu đồ giải thuật điều khiển Robot Delta di chuyển từ điểm đến điểm.
5.2.1 Yêu cầu thiết kế
Để điều khiển delta robot di chuyển từ điểm đến điểm, ta đưa về giải quyết bài tốn điều khiển vị trí cho 3 động cơ bước. Từ tập hợp điểm đó, thơng qua bài tốn động học ngược ta tính được tập hợp góc mà động cơ cần quay. Từ đó, ta tính được số xung cấp tương ứng với từng động cơ. Sau đó, vi điều khiển bắn xung xuống driver để động cơ đi đến vị trí mong muốn. Cơng việc này thực hiện tuần tự và liên tục cho đến khi kết thúc.
5.2.2 Chương trình điều khiển robot delta
Việc đầu tiên của chương trình điều khiển khi vi điều khiển vừa mới được bật lên đó là khởi tạo các ngoại vi cho vi điều khiển, bao gồm:
− Cấp xung clock cho ngoại vi cần sử dụng.
− Ngõ ra cấp xung, chiều cho động cơ.
− Ngõ vào tín hiệu cảm biến.
− Khởi tạo các Timer, chương trình ngắt.
− Khởi tạo module giao tiếp I2C.
Bus I2C (dây giao tiếp) chỉ gồm hai dây và được đặt tên là Serial Clock Line (SCL) và Serial Data Line (SDA). Dữ liệu được truyền đi được gửi qua dây SDA và được đồng bộ với tín hiệu đồng hồ (clock) từ SCL. Tất cả các thiết bị / IC trên mạng I2C được kết nối với cùng đường SCL và SDA như sau:
Hình 5.3 Sơ đồ giao tiếp I2C
Cả hai đường bus I2C (SDA, SCL) đều hoạt động như các bộ lái cực máng hở (open drain). Nó có nghĩa là bất kỳ thiết bị / IC trên mạng I2C có thể lái SDA và SCL xuống mức thấp, nhưng khơng thể lái chúng lên mức cao. Vì vậy, một điện trở kéo lên (khoảng 1 kΩ đến 4,7 kΩ) được sử dụng cho mỗi đường bus, để giữ cho chúng ở mức cao (ở điện áp dương) theo mặc định.
Các thiết bị kết nối với bus I2C được phân loại hoặc là thiết bị Chủ (Master) hoặc là thiết bị Tớ (Slave). Ở bất cứ thời điểm nào thì chỉ có duy nhất một thiết bị Master ở trang thái hoạt động trên bus I2C. Nó điều khiển đường tín hiệu đồng hồ SCL và quyết định hoạt động nào sẽ được thực hiện trên đường dữ liệu SDA. Dữ liệu được truyền giữa thiết bị Master và các thiết bị Slave thông qua một đường dữ liệu SDA duy nhất, thơng qua các chuỗi có cấu trúc gồm các số 0 và 1 (bit). Mỗi chuỗi số 0 và 1 được gọi là giao dịch (transaction).
Sau khi khởi tạo hệ thống. Việc quan trọng tiếp theo đó là đưa robot delta về vị trí Home. Vị trí này được xác định nhờ vào ba cảm biến đo góc nghiêng MPU6050 –
GY521 đặt trên ba cánh tay trên của robot. Sau khi thực hiện set Home cho robot, ta tiến hành điều khiển vị trí cho 3 động cơ.
Đưa về vị trí Home N Cảm biến 1 Set góc Ɵi = a N Cảm biến 2 Set góc Ɵi= a N Cảm biến 3 Set góc Ɵi = a Y Y Y Động cơ 3 dừng Return Động cơ 2 dừng Động cơ 1 dừng
Động cơ 3 quay lên Động cơ 2 quay lên
Động cơ 1 quay lên
5.2.3 Giải thuật điều khiển đưa robot về vị trí Home
Vị trí Home được định nghĩa là vị trí mà ở đó 3 cánh tay trên của robot cùng lệch một góc bằng nhau so với phương ngang. Khi chương trình điều khiển được bật lên nó cần xác định xem robot đã được đưa về vị trí Home hay chưa bằng cách đọc về giá trị 3 góc trên 3 cảm biến. Nếu giá trị 3 cảm biến chưa bằng nhau tại một góc định trước, chương trình sẽ bật động cơ tương ứng điều khiển 3 cánh tay robot di chuyển lên để cảm biến đạt giá trị mong muốn. Chương trình ngắt có nhiệm vụ tắt động cơ sẽ được bật lên khi cơ cấu đạt vị trí Home.
Điều khiển vị trí
goc[i] = new_goc[i] – old_goc[i]
Giá trị Timer[i] = clock/[(2500.goc[i])/9]
Sau khi đưa robot về vị trí Home, ta cần nhập vào điểm mà robot cần di chuyển. Việc tiếp theo chúng cần làm đó là thực hiện bài tốn nội suy để tính ra giá trị các góc và xuất ra số xung tương ứng để điều khiển động cơ.
5.2.4 Giải thuật điều khiển vị trí
Hình 5.5: Lưu đồ giải thuật điều khiển vị trí
Set giá trị Timer; n[i] = 0; N If n[i] < xung[i] Y End N Ngắt Timer[i] Y n[i] = n[i] + 1
Ta chia điểm di chuyển của robot thành một tập hợp điểm, thơng qua bài tốn động học nghịch ta tính được giá trị các góc mà động cơ cần quay thơng qua tập hợp điểm đó. Trong lưu đồ giải thuật, biến goc[i] = new_goc[i] – old_goc[i] chính là giá
trị góc mà động cơ cần tiếp tục quay để đạt được vị trí mong muốn. Biến
𝑇𝑖𝑚𝑒𝑟[𝑖] chính là khoảng thời gian giữa hai lần timer đếm.
Để robot di chuyển mịn hơn, driver điều khiển động cơ được cài đặt số 25600 xung/vòng. Ta tính được số xung cần cấp cho mỗi động cơ là 𝑥𝑢𝑛𝑔[𝑖] = (25600. 𝑔𝑜𝑐[𝑖])/360 = (1250. 𝑔𝑜𝑐[𝑖])/9 .Từ đó, ta tính được giá trị giữa hai lần đếm là 𝑇𝑖𝑚𝑒𝑟[𝑖]
= 𝑐𝑙𝑜𝑐𝑘/[(1250. 𝑔𝑜𝑐[𝑖])/9]. Trong đó, biến 𝑐𝑙𝑜𝑐𝑘 là giá trị chu kì xung clock của
hệ thống. Sau đó ta cài đặt giá trị cần nạp vào cho timer để khi tràn timer nhảy vào chương trình ngắt.
Tiếp đến, nếu số xung hiện tại nhỏ hơn số xung cần cấp thì chương trình sẽ nhảy vào hàm ngắt để bật động cơ quay, đồng thời giá trị biến đếm n[i] sẽ tăng lên 1 đơn vị sau mỗi lần nhảy vào hàm ngắt. Công việc này được lặp đi lặp lại cho đến khi thực hiện đủ số xung cần cấp, động cơ quay đúng giá trị góc mong muốn thì chương trình dừng lại.