- Cải thiện giải thuật để không chỉ nhận dạng được 5 màu mà còn có thể nhận dạng được rất nhiều màu khác, phân biệt được các màu chung một gốc màu như đỏ đậm, đỏ tươi, đỏ thắm, xanh da trời, xanh lam, xanh cô ban,…
- Hiển thị được thông số của các khớp xoay, vận tốc, gia tốc và momen động cơ khi Robot hoạt động.
- Cải tiến chương trình để hệ thống có thể reset tự động và lặp lại quy trình thay cho phương thức reset thủ công như hiện tại.
- Thay vì sử dụng cánh tay Dobot Magician (chủ yếu được phục vụ cho học tập và nghiên cứu) để gắp thả vật thể trong hệ thống, ta sử dụng cánh tay Robot công nghiệp Dobot M1 (hình 6.1) nhằm cải thiện tốc độ hoạt động và nâng cao độ chính xác cho hệ thống.
65
Khoa ĐT CLC – ĐHSPKT TP.HCM
Hình 6. 1 Dobot M1
- Tìm kiếm giải thuật và nghiên cứu các hàm điều khiển để băng chuyền có thể hoạt động đồng thời với cánh tay Robot, tức là cánh tay Robot sẽ chạy theo gắp các vật thể trên băng chuyền khi băng chuyền đang hoạt động (chạy liên tục không dừng), từ đó nâng cao năng suất hệ thống.
66
Khoa ĐT CLC – ĐHSPKT TP.HCM
TÀI LIỆU THAM KHẢO Tiếng Việt
[1] PGS.TS Nguyễn Trường Thịnh (2014), “Giáo trình Kỹ thuật Robot”, Nhà xuất bản Đại học Quốc gia Thành phố Hồ Chí Minh
[2] TS. Nguyễn Thanh Hải (2014), “Giáo trình Xử lý ảnh”, Nhà xuất bản Đại học Quốc gia Thành phố Hồ Chí Minh
[3] Phạm Trần Lam Hải, Trần Thanh Hùng, Nguyễn Văn Khanh, Lưu Trọng Hiếu (2015), “Ứng dụng xử lý ảnh số trong việc theo dõi sự chuyển động của các Robot di động”, Tạp chí Khoa học Trường Đại học Cần Thơ
[4] Nguyễn Văn Long (2016), “Ứng dụng Xử lý ảnh trong thực tế với thư viện OpenCV”
[5] Võ Đức Khánh, Hoàng Văn Kiếm (2008), “Giáo trình xử lý ảnh số”, Nhà xuất bản Đại học Quốc gia Thành phố Hồ Chí Minh
[6] Nguyễn Tiến Phúc (2017), “Xử lý ảnh trong phân loại sản phẩm theo màu sắc”,
Bài báo nghiên cứu khoa học Trường Đại học Sao Đỏ
[7] Nguyễn Quang Hoan (2006), “Xử lý ảnh”, Lưu hành nội bộ Học viện công nghệ bưu chính viễn thông
[8] Nguyễn Thị Lan (2011), “Tìm hiểu phương pháp BPR cho bài toán tìm xương của ảnh”, Đồ án tốt nghiệp Trường Đại học Dân lập Hải Phòng
[9] Chí Trương (2013), “Một vài khái niệm cơ bản về xử lý ảnh số”, Wordpress
vtct.wordpress.com/2013/06/19/mot-vai-khai-niem-co-ban-ve-anh-so-digital-image/
[10] Việt Anh (2018), “Ảnh số và các không gian màu trong xử lý ảnh”, Blog Việt Anh
blog.vietanhdev.com/posts/computer-vision/2018-09-19-anh-so-va-cac-khong-gian- mau-trong-xu-ly-anh/
[11] Tạ Hải (2017), “Giới thiệu khái quát về ảnh số”, Blog Lập trình Matlab
www.matlabthayhai.info/2015/11/bai-1-gioi-thieu-khai-quat-ve-anh-so.html
[12] Team Việt Dev (2018), “OpenCV là gì? Ứng dụng của nó trong thế giới thực”,
Blog Team Việt Dev
https://teamvietdev.com/opencv-la-gi-ung-dung-opencv-trong-the-gioi-thuc/
[13] Nguyễn Nghĩa (2016), “Một cái nhìn tổng quản về Visual Studio IDE”,
EITGuide
67
Khoa ĐT CLC – ĐHSPKT TP.HCM
Tiếng Anh
[1] Bill Green (2002), “Canny Edge Detection”, DocsOpenCV.com
[2] Krutika Bapat (2019), “Hough Transform with OpenCV”, LearnOpenCV.com
[3] Andreas Richtsfeld, Thomas Morwald, Johann Prankl, Michael Xillich and Markus Vincze (2012), “Segmentation of Unknown Objects in Indoor Environments”, IEEE/RSJ International Conference on Intelligent Robots and Systems
[4] R.B Rusu, A. Holzbach, M. Beetz (2009), “Detecting and Segmenting Objects for Mobile Manipulation”, S3DV Workshop of the 12th International Conference on Computer Vision (ICCV)
[5] Gonzalez, RC and Woods, RE (2008), “Digital Image Processing”, Pearson Prentice Hall, Upper Saddle River
[6] Bhabatosh Chanda, Dwijesh Dutta Maumder (2001), “Digital Image Processing and Analysis”, Prentice Hall of India
[7] Wilhelm Burger and Mark J. Burge (2009), “Principles of Digital Image Processing”, Fundamental Techniques, Sprigner
[8] Support Dobot, “Dobot Magician User Guide”, Dobot.cc
https://download.dobot.cc/product-manual/dobot-magician/pdf/V1.7.0/en/Dobot- Magician-User-Guide-V1.7.0.pdf
[9] Support Dobot, “Dobot Magician Demo Description”, Dobot.cc
https://download.dobot.cc/product-manual/dobot-magician/pdf/en/dobot-magician- demo-description.pdf
[10] Support Dobot, “Dobot Magician API Description”, Dobot.cc
http://download.dobot.cc/development-protocol/dobot-magician/pdf/api/en/Dobot- Magician-API-DescriptionV1.2.3.pdf
68
Khoa ĐT CLC – ĐHSPKT TP.HCM
PHỤ LỤC
Chương trình điều khiển:
Lấy màu mẫu: Color_Sample import cv2
cap = cv2.VideoCapture(0) cap.set(3, 320)
cap.set(4, 240)
while True:
# doc 1 anh tu camera frame = cap.read()[1] with open('specs', 'r') as f: content = f.read() specs = content.split() specs = list(map(int, specs)) f.close()
# cat khung hinh lay phan chua bang bang chuyen
img = frame[frame.shape[0]//3:frame.shape[0]//3*2, 98:-98] red = img[specs[0]:specs[2], specs[1]:specs[3]]
green = img[specs[4]:specs[6], specs[5]:specs[7]] blue = img[specs[8]:specs[10], specs[9]:specs[11]] yellow = img[specs[12]:specs[14], specs[13]:specs[15]]
cv2.imshow("red", red) cv2.imshow("green", green) cv2.imshow("blue", blue) cv2.imshow("yellow", yellow) cv2.imshow("adad", img) if cv2.waitKey(2) & 0xFF == 27: cv2.imwrite("photos/red.png", red)
69 Khoa ĐT CLC – ĐHSPKT TP.HCM cv2.imwrite("photos/green.png", green) cv2.imwrite("photos/blue.png", blue) cv2.imwrite("photos/yellow.png", yellow) break cv2.destroyAllWindows()
Nhận diện hình vuông: Rect_Recognition import cv2
importnumpy as np
from random importrandint
from math import atan, pi, fabs, sqrt, tan
from matplotlib import pyplot
from mpl_toolkits.mplot3dimportAxes3D
rect_width = 31 delta_dir = 10*pi/180
def get_distance_p2l(point, line):
'''/
Calculate distance from point to line @param:
point: coordination of the point format: [x, y]
line: line's index in the list of all lines format: integer
@ return: distance from point to line format: float
'''
a, b, c = get_fomular_of_line(line)
70
Khoa ĐT CLC – ĐHSPKT TP.HCM
def get_distance_l2l(line1, line2):
'''
Calculate distance from line1 to line2 @param:
lineX: a list contains center, direction and lenght of lineX format: [center, direction, lenght]
@return:
distance from line1 to line2 format: float
'''
return (get_distance_p2l(line1[0], line2[:2]) + get_distance_p2l(line2[0], line1[:2]))/2
def get_distance_p2p(p1, p2):
'''
Calculate distance from point 'p1' to point 'p2' @param:
pX: coordination of pX format: float
'''
return sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2)
def get_intersection(line1, line2):
'''
Find intersection's coordinate of line1 nad line2 @param:
lineX: a list contains center, direction and lenght of lineX format: [center, direction, lenght]
@return:
intersection's coordinate of line1 nad line2 format: [x, y]
71 Khoa ĐT CLC – ĐHSPKT TP.HCM a1, b1, c1 = get_fomular_of_line(line1) a2, b2, c2 = get_fomular_of_line(line2) if a1 == 0: y = fabs(c1/b1) x = fabs((y*b2 + c2)/a2) elif a2 == 0: y = fabs(c2/b2) x = fabs((y*b1 + c1)/a1) else: y = fabs((c1-c2)/(b1-b2)) x = fabs((b1*y+c1)/a1) return x, y
def get_center_rect(rect, ls_of_cen_dir_len):
'''
Find center's coordinate of rectangle @param:
rect: a list contains pair of horizontal lines and pair of verticac lines format [[line_v1, line_v2], [line_h1, lin_h2]]
ls_of_cen_dir_len: a list contains center, direction and lenght of all lines format: [[center1, direction1, lenght1], [center2, direction2, lenght2], ...] @return: center's coordinate of rectangle
format: int [x, y] '''
ls_of_peaks = [] for line_v in rect[0]: for line_h in rect[1]:
inter = get_intersection(ls_of_cen_dir_len[line_v], ls_of_cen_dir_len[line_h]) ls_of_peaks.append(inter)
72
Khoa ĐT CLC – ĐHSPKT TP.HCM
y = (ls_of_peaks[0][1] + ls_of_peaks[1][1] + ls_of_peaks[2][1] + ls_of_peaks[3][1])/4 sum_direction = ls_of_cen_dir_len[rect[1][0]][1] + ls_of_cen_dir_len[rect[0][0]][1] + pi/2
sum_direction /= 2
return (int(x), int(y)), sum_direction
def is_between_2lines(points, lines):
'''
Check if points are between 2 lines @param:
points: coordination of points format: float [x, y]
lines: formulars 'ax + by + c = 0' of 2 lines format: float [a, b, c]
@return: boolean value ''' for c in points: if (c[0]*lines[0][0]+c[1]*lines[0][1]+lines[0][2]) * (c[0]*lines[1][0] + c[1]*lines[1][1] + lines[1][2]) >= 0: return False return True def get_fomular_of_line(line): '''
Calculate formular of line @param:
line: a list contains center, direction and lenght of line format: [center, direction, lenght]
@return:
formular of line 'ax + by +c = 0' format: float[a, b, c]
73 Khoa ĐT CLC – ĐHSPKT TP.HCM ''' if line[1] == 0: a = 0.0 b = 1.0 else: a = 1.0 b = a * tan(line[1]+pi/2) c = -(a*line[0][0] + b*line[0][1]) return a, b, c def is_rect(v1, v2, h1, h2): '''
Check if for line v1, v2, h1, h2 are parts of real rect @param:
v1, v2, h1, h2: lists contain center, direction and lenght of v1, v2, h1, h2 format: float[center, direction, lenght]
@return:
boolean value '''
line_v = (get_fomular_of_line(v1), get_fomular_of_line(v2)) center_h = (h1[0], h2[0])
line_h = (get_fomular_of_line(h1), get_fomular_of_line(h2)) center_v = (v1[0], v2[0])
ifnot is_between_2lines(center_h, line_v): return False
elifnot is_between_2lines(center_v, line_h): return False
else: return True
def get_dist_3d_p2p(color, center):
74
Khoa ĐT CLC – ĐHSPKT TP.HCM
def get_nearest_color(color_mean, color_center):
nearest_dist = get_dist_3d_p2p(color_mean, color_center[0]) nearest_color = 0
for idx, center in enumerate(color_center[1:], 1): dist = get_dist_3d_p2p(color_mean, center) if dist < nearest_dist:
nearest_dist = dist nearest_color = idx return nearest_color
def detect_color(ls_of_rects, color_center, img):
# color_ls = ['red', 'green', 'blue', 'yellow', 'black']
for idx, rect in enumerate(ls_of_rects):
rect_img = img[rect[0][1]-10:rect[0][1]+10, rect[0][0]-10:rect[0][0]+10] cv2.imshow("rect", rect_img)
color_mean = rect_img.mean(axis=0).mean(axis=0).astype(int) color_id = get_nearest_color(color_mean, color_center)
rect.append(color_id)
def detect_pair_closed_parallel_lines(cluster, ls_of_cen_dir_len):
'''
Find pairs of close parallel lines in cluster of lines @param:
cluster: a list contains horizontal lines and vertical lines format: [[h1, h2, ...], [v1, v2, ...]]
ls_of_cen_dir_len: a list contains center, direction, lenght of all lines format: float[[center, direction, lenght], ...]
@return: a set contains list of couples vertical lines's index and a list os couples horizontal lines's index
format: '''
75
Khoa ĐT CLC – ĐHSPKT TP.HCM for idx, i in enumerate(cluster[0][:-1]):
for j in cluster[0][idx+1:]:
dist_l2l = get_distance_l2l(ls_of_cen_dir_len[i], ls_of_cen_dir_len[j]) if dist_l2l >= rect_width*0.85 and dist_l2l <= rect_width*1.15: ls_of_couple_v.append((i, j))
ls_of_couple_h = []
for idx, i in enumerate(cluster[1][:-1]): for j in cluster[1][idx+1:]:
dist_l2l = get_distance_l2l(ls_of_cen_dir_len[i], ls_of_cen_dir_len[j]) if dist_l2l >= rect_width*0.85 and dist_l2l <= rect_width*1.15: ls_of_couple_h.append((i, j))
return (ls_of_couple_v, ls_of_couple_h)
def detect_real_rect(ls_of_cen_dir_len, clusters_of_paral_vert_lines):
'''
@param:
ls_of_cen_dir_len: a list contains center, direction, lenght of all lines format: float[[center, direction, lenght], ...]
clusters_of_paral_vert_lines: list of all clusters format: [[[h1, h2, ...], [v1, v2, ...]], ...] @return:
ls_of_real_rects: a list contains center of real rects format: [[(x0, y0), dir0], [(x1, y1), dir1], ...] '''
ls_of_center_rects = [] ls_of_real_rects = []
for cluster in clusters_of_paral_vert_lines:
ls_of_pairs = detect_pair_closed_parallel_lines(cluster, ls_of_cen_dir_len)
for v in ls_of_pairs[0]: for h in ls_of_pairs[1]:
76
Khoa ĐT CLC – ĐHSPKT TP.HCM if is_rect(ls_of_cen_dir_len[v[0]], ls_of_cen_dir_len[v[1]],
ls_of_cen_dir_len[h[0]], ls_of_cen_dir_len[h[1]]):
# check if v and h are parts of a real rect
vh = [v, h]
# compute
center, direction = get_center_rect(vh, ls_of_cen_dir_len)
# cluster duplicated rects into groups
added = False
for i in ls_of_center_rects:
if get_distance_p2p(center, i[0][0]) < rect_width*0.5: i.append((center, direction))
added = True
break
ifnot added:
ls_of_center_rects.append([(center, direction)])
# compute average center of duplicated rect group
for c in ls_of_center_rects: x_sum = 0 y_sum = 0 dir_sum = 0 for each in c: x_sum += each[0][0] y_sum += each[0][1] dir_sum += each[1] ls_of_real_rects.append([(int(x_sum/len(c)), int(y_sum/len(c))), dir_sum/len(c)/pi*180]) return ls_of_real_rects
def find_in_neighbour(line, ls_of_cen_and_dir):
'''
Find all lines which locate in neighbourhood around line @param:
77
Khoa ĐT CLC – ĐHSPKT TP.HCM
line: a list contains center, direction, lenght of line format: float[center, direction, lenght]
ls_of_cen_and_dir: a list contains center, direction, lenght of all lines format: [[center0, direction0, lenght0], [center1, direction1, lenght1], ...] @return:
list of horizontal lines and vertical lines format: [[h1, h2, ...], [v1, v2, ...]] '''
center = line[0] direction = line[1] res = [[],[]]
for i, each in enumerate(ls_of_cen_and_dir): distance = get_distance_p2p(center, each[0])
if distance < rect_width*1.3 and distance >= rect_width*0.3: if fabs(direction-each[1]) < delta_dir:
res[0].append(i)
elif fabs(fabs(direction-each[1])-pi/2) < delta_dir: res[1].append(i)
elif distance < rect_width*0.3:
if fabs(direction-each[1]) < delta_dir: res[0].append(i)
return res
def get_center_direction_len(ls_of_lines):
'''
Calculate center, direction, lenght of all detected lines @param:
ls_of_lines: list of all lines defined by 2 end-points format: [(x1, y1, x2, y2), (x1, y1, x2, y2), ...] @return:
78
Khoa ĐT CLC – ĐHSPKT TP.HCM
format: [[center1, direction1, lenght1], [center2, direction2, lenght2], ...] '''
res = list()
for line in ls_of_lines: x1, y1, x2, y2 = line[0] p1 = (x1, y1) p2 = (x2, y2) line_center = [(x1+x2)//2, (y1+y2)//2] lenght = get_distance_p2p(p1, p2) if x1 == x2: line_direction = pi/2 elif y1 == y2: line_direction = 0 else: line_direction = atan((y1-y2)/(x1-x2)) res.append([line_center, line_direction, lenght])
return res
def find_clusters_of_paral_vert_lines(ls_of_lines):
'''
Find the neighbours of each line in ls_of_lines @param:
ls_of_lines: list of all lines defined by 2 end-points format: [(x1, y1, x2, y2), (x1, y1, x2, y2), ...] @return:
clusters_of_paral_vert_lines: list of all clusters format: [[[h1, h2, ...], [v1, v2, ...]], ...]
ls_of_cen_dir_len: a list contains center, direction, lenght of all lines format: float[[center, direction, lenght], ...]
'''
79
Khoa ĐT CLC – ĐHSPKT TP.HCM ls_of_cen_dir_len = get_center_direction_len(ls_of_lines)
for line in ls_of_cen_dir_len: if line[2] > rect_width*0.2:
# if lenght of line is longer than rect_width*0.2
clusters_of_paral_vert_lines.append(find_in_neighbour(line, ls_of_cen_dir_len)) return clusters_of_paral_vert_lines, ls_of_cen_dir_len
def auto_canny(image, sigma=0.33):
# compute the median of the single channel pixel intensities
v = np.median(image)
# apply automatic Canny edge detection using the computed median
lower = int(max(0, (1.0 - sigma) * v)) upper = int(min(255, (1.0 + sigma) * v)) edged = cv2.Canny(image, lower, upper)
# return the edged image
return edged
def detect_rects(img, color_center):
'''
@return:
ls_of_real_rects: a list contains center and directions of real rects format: [[(x0, y0), dir0], [(x1, y1), dir1], ...]
'''
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) blur = cv2.GaussianBlur(gray, (5, 5), 0)
edged = auto_canny(blur)
# edged = cv2.Canny(blur, 100, 200)
cv2.imshow("canny", edged)
80
Khoa ĐT CLC – ĐHSPKT TP.HCM ls_of_real_rects = []
if lines is not None:
# print("num of lines:", len(lines))
clusters_of_paral_vert_lines, ls_of_cen_dir_len = find_clusters_of_paral_vert_lines(lines)
# print("num of rects:", len(clusters_of_paral_vert_lines))
ls_of_real_rects = detect_real_rect(ls_of_cen_dir_len, clusters_of_paral_vert_lines) detect_color(ls_of_real_rects, color_center, img)
for idx, rect in enumerate(ls_of_real_rects): if rect[-1] == 4:
ls_of_real_rects.pop(idx) return ls_of_real_rects
Chương trình chính: Main import cv2
from time import sleep, time
from rect_regconition import detect_rects, get_distance_p2p
import threading
import DobotDllType as dType
# khoang cach gioi han nho nhat giua 2 vat the
ref_new = 10
# khoang cach tu tam camera den truc x cua robot
d_robot_cam = 330.5
# toc do bang tai (mm/s)
mm_per_sec = 40
# flag dieu khien ket thuc thread 'arm'
81
Khoa ĐT CLC – ĐHSPKT TP.HCM
# list cac vat the dang duoc theo doi
ls_obj = []
class Rectangle(object):
'''
Moi vat the duoc dinh nghia bang 1 object Rectangle '''
def __init__(self, location, orientation, color):
self.location = location self.orientation = orientation self.color = color
def is_new_obj(rect_center, ls_obj):
'''
Kiem tra xem vat the o khung hinh hien tai va vat the o khung hinh truoc do co phai la 1 vat the khong?
Kiem tra toi da 6 vat the '''
if len(ls_obj) > 6: start_idx = -6
else:
start_idx = -len(ls_obj)
for idx in range(start_idx, 0, 1):
dist = get_distance_p2p(ls_obj[idx].location, rect_center)
# khoang cach tu vat the dang xet o khung hinh hien tai den 6 vat the o khung hinh truoc do nho hon ref_new khong?
if dist < ref_new: return False return True
def get_real_pos(img_pos, img_shape, delta_s):
82
Khoa ĐT CLC – ĐHSPKT TP.HCM
Chuyen doi tu toa do vat the tren anh sang toa do robot Ty le chuyen doi: 25 pixel ~ 30 mmm
'''
return [(img_pos[0]-img_shape[1]/2)*25/30, (img_pos[1]-img_shape[0]/2)*25/31 + delta_s]
def convert_base(ls_of_rects, img_shape, delta_s):
'''
Chuyen doi toa do danh sach vat the tren anh sang toa do robot '''
for rect in ls_of_rects:
rect[0] = get_real_pos(rect[0], img_shape, delta_s)
def get_color_center():
'''
Tinh sample color cua 5 mau '''
color_ls = ['red', 'green', 'blue', 'yellow', 'black'] color_center = []
for color in color_ls:
img = cv2.imread('photos/'+color+'.png')
# tinh gia tri trung binh cua cac diem anh tren 3 thang mau RGB
p = img.mean(axis=0).mean(axis=0).astype(int)
# chuyen doi tu dinh dang numpy sang dinh dang list
color_center.append(list(p)) return color_center [ [x0, y0, z0], [], [], ...] def main(threadname): # mo camera so 0 cap = cv2.VideoCapture(0)
83
Khoa ĐT CLC – ĐHSPKT TP.HCM
# setup ty le khung hinh
cap.set(3, 320) cap.set(4, 240) # setup FPS cap.set(5, 30) sleep(1) last_time = 0 timef2f = 0
# tinh sample color cua 5 mau
color_center = get_color_center()
while True:
start_time = time() # doc 1 anh tu camera
frame = cap.read()[1]
# cat khung hinh lay phan chua bang bang chuyen
img = frame[frame.shape[0]//3:frame.shape[0]//3*2, 98:-98]
# tim tat ca hinh vuong trong img
# ls_of_rects = [ [[x, y], orientation, color] , [], ...]
ls_of_rects = detect_rects(img, color_center)
# calculate time spent by rect_regconition # print("num of detected rect:", len(ls_of_rects)) # print("eslaped time:", time() - start_time) # show rect_regconition results
color = [(255, 0, 255), (255, 0, 0), (0, 0, 255)] for rect in ls_of_rects:
84
Khoa ĐT CLC – ĐHSPKT TP.HCM
cv2.putText(img,str(int(rect[1])),rect[0],cv2.FONT_HERSHEY_SIMPLEX, 0.5,(0,255,0),1,cv2.LINE_AA)
cv2.circle(img, (img.shape[1]//2, img.shape[0]//2), 2, color[2], thickness=1) cv2.imshow('origin', img)
# convert coordinate base from image base to robot base
delta_s = (time()-start_time) * mm_per_sec
# print('time1:', time()-start_time)
convert_base(ls_of_rects, img.shape, delta_s)
# khai bao ls_obj la global de co the sua doi du lieu
global ls_obj
# can chinh lai toa do cac vat the
delta_s = (time()-last_time) * mm_per_sec
# print('delta:', delta_s)
last_time = time() for obj in ls_obj:
obj.location[1] += delta_s
# cap nhat danh sach cac vat the ls_obj
for rect in ls_of_rects:
if is_new_obj(rect[0], ls_obj): end_idx = -len(ls_obj)-1 if end_idx < -7:
end_idx = -7 inserted_flag = False
for i in range(-1, end_idx, -1):
if rect[0][1] <= ls_obj[i].location[1]: if i == -1:
85
Khoa ĐT CLC – ĐHSPKT TP.HCM else:
ls_obj.insert(i, Rectangle(rect[0], rect[1], rect[2])) inserted_flag = True
break
if not inserted_flag:
ls_obj.insert(end_idx+1, Rectangle(rect[0], rect[1], rect[2])) if cv2.waitKey(1) & 0xFF == 27:
global end_thread end_thread = True break
cv2.destroyAllWindows()
def pick_up(api, location, orientation, color, cur_pos_wh, size_wh):
# vi tri goc cua kho
warehouse_base = [-88, 198, -42]
# dieu khien arm den vi tri x, y, z
x = location[0] + 217 y = d_robot_cam - location[1] z = 12 print(x,y,z) dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode, x, y, z, 0, isQueued=1) dType.SetEndEffectorSuctionCup(api, 1, 1, isQueued=1)
# dieu khien arm den vi tri trong trong kho chua
x = cur_pos_wh[color][0]*35 + warehouse_base[0] + 40*color y = cur_pos_wh[color][1]*35 + warehouse_base[1]
z = cur_pos_wh[color][2]*26 + warehouse_base[2]
dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode, x, y, z, orientation, isQueued=1)[0]
86
Khoa ĐT CLC – ĐHSPKT TP.HCM dType.SetEndEffectorSuctionCup(api, 1, 0, isQueued=1)
# luu chi so cua lenh cuoi cung
lastIndex = dType.SetWAITCmd(api, 0.2, isQueued=1)[0]
# cap nhat tri trong trong kho chua
if cur_pos_wh[color][0] == size_wh[0]-1: cur_pos_wh[color][0] = 0 if cur_pos_wh[color][1] == size_wh[1]-1: cur_pos_wh[color][1] = 0 # if cur_pos_wh[color][2] < size_wh[2]-1: cur_pos_wh[color][2] += 1 else: cur_pos_wh[color][1] += 1 else: cur_pos_wh[color][0] += 1
# doi robot thuc hien xong qua trinh, sau do ket thuc
cur_cmd = dType.GetQueuedCmdCurrentIndex(api)[0]
while lastIndex > cur_cmd:
# print(cur_cmd)
dType.dSleep(10)
cur_cmd = dType.GetQueuedCmdCurrentIndex(api)[0]
def arm(threadname):
'''
Thread dieu khien robot arm '''
# kich thuoc kho chua hang doi voi tung mau, dinh dang [x, y, z]
87
Khoa ĐT CLC – ĐHSPKT TP.HCM
# vi tri hien tai cua tung mau trong kho hang: Red Green Blue Yellow
cur_pos_wh = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]] CON_STR = { dType.DobotConnect.DobotConnect_NoError: "DobotConnect_NoError", dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound", dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied"} # Load Dll api = dType.load() # Connect Dobot state = dType.ConnectDobot(api, "", 115200)[0] print("Connect status:",CON_STR[state]) dType.SetHOMEParams(api, 230, 0, 50, 0, isQueued=1) dType.SetPTPCoordinateParams(api, 150, 200, 200, 200, isQueued=1) dType.SetPTPCommonParams(api, 100, 100, isQueued=1) dType.SetPTPJumpParams(api, 60, 150, isQueued=1) dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode, 200, 0, 50, 0, isQueued=1)
lastIndex = dType.SetHOMECmd(api, temp = 0, isQueued = 1)[0] cur_cmd = dType.GetQueuedCmdCurrentIndex(api)[0]
while lastIndex > cur_cmd: dType.dSleep(10)
cur_cmd = dType.GetQueuedCmdCurrentIndex(api)[0]
global mm_per_sec
dType.SetQueuedCmdClear(api) dType.SetQueuedCmdStartExec(api)
88
Khoa ĐT CLC – ĐHSPKT TP.HCM f = open('step', 'r')
STEP_PER_CIRCLE = float(f.read().strip('\n')) f.close()
# so buoc de truc quay du 1 vong
# STEP_PER_CIRCLE = 17400 #16625 # chu vi truc quay bang chuyen
MM_PER_CIRCLE = 3.1415926535898 * 36.0
# so buoc trong 1 giay
pulse_per_sec = mm_per_sec * STEP_PER_CIRCLE / MM_PER_CIRCLE
# dieu khien stepper
dType.SetEMotor(api, 0, 1, int(pulse_per_sec), isQueued=1) while state == dType.DobotConnect.DobotConnect_NoError:
# print('number:', len(ls_obj)) if ls_obj: # print("ab") if ls_obj[0].location[1] >= d_robot_cam-20: while True: try: if ls_obj[0].location[1] >= d_robot_cam-100: print(cur_pos_wh)
# neu kho hang cua mau dang xet chua day
if cur_pos_wh[ls_obj[0].color][2] < size_wh[2]:
# dung bang chuyen
dType.SetEMotor(api, 0, 1, int(0), isQueued=1) mm_per_sec = 0
# gap vat the tu bang chuyen vao kho
pick_up(api, ls_obj[0].location, ls_obj[0].orientation, ls_obj[0].color, cur_pos_wh, size_wh)
89
Khoa ĐT CLC – ĐHSPKT TP.HCM
# xoa vat the vua gap
ls_obj.pop(0) else:
break
except Exception: break
# bang chuyen tiep tuc chay
f = open('step', 'r')
STEP_PER_CIRCLE = float(f.read().strip('\n')) f.close()
# chu vi truc quay bang chuyen
MM_PER_CIRCLE = 3.1415926535898 * 36.0
# so buoc trong 1 giay
mm_per_sec = 40
pulse_per_sec = mm_per_sec * STEP_PER_CIRCLE / MM_PER_CIRCLE dType.SetEMotor(api, 0, 1, int(pulse_per_sec), isQueued=1)
if end_thread: dType.SetEMotor(api, 0, 1, 0, isQueued=1) print('End process') dType.DisconnectDobot(api) break if __name__ == '__main__':
thread_main = threading.Thread(target=main, args=('Thread-1', )) thread_main.start()
thread_arm = threading.Thread(target=arm, args=('Thread-2', ))