Skip to content

Commit c5056b2

Browse files
wangWkingwangWking
committed
优化吸泵过早释放问题,区域放置点改为角度控制
1 parent 1ed40c3 commit c5056b2

File tree

1 file changed

+43
-46
lines changed

1 file changed

+43
-46
lines changed

‎main.py‎

Lines changed: 43 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -112,11 +112,12 @@ def _init_variable(self):
112112
[16.96, -6.85, -54.93, -19.68, 89.47, 12.83], # color gripper init point
113113
]
114114
# 移动坐标（形状、特征点、yolvo5、二维码）
115-
self.move_coords = [
116-
[32, -228.3, 201.6, -168.07, -7.17, -92.56], # D Sorting area
117-
[266.5, -219.7, 209.3, -170, -3.64, -94.62], # C Sorting area
118-
[253.8, 236.8, 224.6, -170, 6.87, -77.91], # A Sorting area
119-
[35.9, 235.4, 211.8, -169.33, -9.27, 88.3], # B Sorting area
115+
# 使用角度到达放置点
116+
self.move_coords_to_angles = [
117+
[-67.58, -28.65, -86.39, 34.18, 97.64, -64.95], # D Sorting area
118+
[-24.69, -54.58, -36.65, 9.31, 90.35, -20.74], # C Sorting area
119+
[58.178, -55.45, -28.74, 3.51, 87.8, 46.14], # A Sorting area
120+
[100.28, -5.27, -111.7, 36.56, 98.78, -77.51], # B Sorting area
120121
]
121122
# The internal parameter matrix of the camera
122123
self.camera_matrix = np.array([
@@ -419,12 +420,12 @@ def device_coord(self):
419420
[17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
420421
[16.96, -6.85, -54.93, -19.68, 89.47, 12.83], # color gripper init point
421422
]
422-
# 移动坐标（形状、特征点、yolvo5、二维码）
423-
self.move_coords = [
424-
[32, -228.3, 201.6, -168.07, -7.17, -92.56], # D Sorting area
425-
[266.5, -219.7, 209.3, -170, -3.64, -94.62], # C Sorting area
426-
[253.8, 236.8, 224.6, -170, 6.87, -77.91], # A Sorting area
427-
[35.9, 235.4, 211.8, -169.33, -9.27, 88.3], # B Sorting area
423+
# 使用角度移动到放置点（形状、特征点、yolvo5、二维码）
424+
self.move_coords_to_angles = [
425+
[-67.58, -28.65, -86.39, 34.18, 97.64, -64.95], # D Sorting area
426+
[-24.69, -54.58, -36.65, 9.31, 90.35, -20.74], # C Sorting area
427+
[58.178, -55.45, -28.74, 3.51, 87.8, 46.14], # A Sorting area
428+
[100.28, -5.27, -111.7, 36.56, 98.78, -77.51], # B Sorting area
428429
]
429430

430431
def connect_mycobot(self):
@@ -1674,13 +1675,11 @@ def moved(self, x, y, yaw_degrees):
16741675

16751676
# send coordinates to move mycobot
16761677
if func == 'QR code recognition' or func == '二维码识别':
1677-
self.move_coords = [
1678-
# [32, -228.3, 201.6, -168.07, -7.17, -92.56], # D Sorting area
1679-
[26.8, -230.8, 229.3, -174.56, -8.33, -92.74], # D Sorting area
1680-
[266.5, -219.7, 209.3, -170, -3.64, -94.62], # C Sorting area
1681-
[253.8, 236.8, 224.6, -170, 6.87, -77.91], # A Sorting area
1682-
# [35.9, 235.4, 211.8, -169.33, -9.27, 88.3], # B Sorting area
1683-
[50.3, 237.1, 231.3, 178.18, -0.95, 87.9], # B Sorting area
1678+
self.move_coords_to_angles = [
1679+
[-60.9, 1.75, -1098.45, 24.69, 90.17, -58.62], # D Sorting area
1680+
[-24.69, -54.58, -36.65, 9.31, 90.35, -20.74], # C Sorting area
1681+
[58.178, -55.45, -28.74, 3.51, 87.8, 46.14], # A Sorting area
1682+
[99.58, -5.0, -92.9, 6.32, 87.89, -77.78], # B Sorting area
16841683
]
16851684
self.myCobot.send_coords(
16861685
[self.home_coords[0] + x, self.home_coords[1] + y, 240, 178.99, -3.78, -62.9], 100,
@@ -1699,12 +1698,12 @@ def moved(self, x, y, yaw_degrees):
16991698
else:
17001699
yaw_degrees = yaw_degrees
17011700
yaw_degrees_opt = yaw_degrees + 5
1702-
# 移动坐标
1703-
self.move_coords = [
1704-
[30.3, -214.9, 302.3, -169.77, -8.64, -91.55], # D Sorting area
1705-
[240.3, -202.2, 317.1, -152.12, -10.15, -95.73], # C Sorting area
1706-
[244.5, 193.2, 330.3, -160.54, 17.35, -74.59], # A Sorting area
1707-
[33.2, 205.3, 322.5, -170.22, -13.93, 92.28], # B Sorting area
1701+
# 移动角度
1702+
self.move_coords_to_angles = [
1703+
[-59.15, 8.17, -81.56, 5.97, 93.86, -58.09], # D Sorting area
1704+
[-23.9, -33.92, -38.75, 9.66, 90.08, -20.83], # C Sorting area
1705+
[54.58, -42.89, -11.16, -12.3, 90.61, 42.01], # A Sorting area
1706+
[103.18, 9.75, -75.32, -11.16, 96.76, -78.83], # B Sorting area
17081707
]
17091708
if yaw_degrees_opt > 170:
17101709
yaw_degrees_opt = 170
@@ -1741,11 +1740,11 @@ def moved(self, x, y, yaw_degrees):
17411740

17421741
elif func == 'object recognition' or func == '物体识别' or func == 'Color recognition grip' or func == '颜色识别 夹爪':
17431742
# 移动坐标
1744-
self.move_coords = [
1745-
[30.3, -214.9, 302.3, -169.77, -8.64, -91.55], # D Sorting area
1746-
[240.3, -202.2, 317.1, -152.12, -10.15, -95.73], # C Sorting area
1747-
[244.5, 193.2, 330.3, -160.54, 17.35, -74.59], # A Sorting area
1748-
[33.2, 205.3, 322.5, -170.22, -13.93, 92.28], # B Sorting area
1743+
self.move_coords_to_angles = [
1744+
[-59.15, 8.17, -81.56, 5.97, 93.86, -58.09], # D Sorting area
1745+
[-23.9, -33.92, -38.75, 9.66, 90.08, -20.83], # C Sorting area
1746+
[54.58, -42.89, -11.16, -12.3, 90.61, 42.01], # A Sorting area
1747+
[103.18, 9.75, -75.32, -11.16, 96.76, -78.83], # B Sorting area
17491748
]
17501749
self.myCobot.set_gripper_mode(0)
17511750
time.sleep(0.5)
@@ -1758,24 +1757,22 @@ def moved(self, x, y, yaw_degrees):
17581757
# close gripper
17591758
self.gripper_off()
17601759
elif func == 'shape recognition' or func == 'Keypoints' or func == '形状识别' or func == '特征点识别' or func == 'yolov5':
1761-
self.move_coords = [
1762-
# [32, -228.3, 201.6, -168.07, -7.17, -92.56], # D Sorting area
1763-
[26.8, -230.8, 229.3, -174.56, -8.33, -92.74], # D Sorting area
1764-
[266.5, -219.7, 209.3, -170, -3.64, -94.62], # C Sorting area
1765-
[253.8, 236.8, 224.6, -170, 6.87, -77.91], # A Sorting area
1766-
# [35.9, 235.4, 211.8, -169.33, -9.27, 88.3], # B Sorting area
1767-
[50.3, 237.1, 231.3, 178.18, -0.95, 87.9], # B Sorting area
1760+
self.move_coords_to_angles = [
1761+
[-60.9, 1.75, -1098.45, 24.69, 90.17, -58.62], # D Sorting area
1762+
[-24.69, -54.58, -36.65, 9.31, 90.35, -20.74], # C Sorting area
1763+
[58.178, -55.45, -28.74, 3.51, 87.8, 46.14], # A Sorting area
1764+
[99.58, -5.0, -92.9, 6.32, 87.89, -77.78], # B Sorting area
17681765
]
17691766
self.myCobot.send_coords([x, y, 230, -173.84, -0.14, -74.37], 100, 1)
17701767
time.sleep(2.5)
17711768
self.myCobot.send_coords([x, y, self.camera_z, -173.84, -0.14, -74.37], 100, 1) #
17721769
time.sleep(3)
17731770
else:
1774-
self.move_coords = [
1775-
[28.9, -226, 246, -171.13, -3.94, -92.37], # D Sorting area
1776-
[253.3, -216.1, 257, -163.12, -6.12, -95.27], # C Sorting area
1777-
[241.8, 219.5, 270.6, -168.47, 10.42, -76.84], # A Sorting area
1778-
[37.8, 233, 251.4, -170.6, -6.75, 88.53], # B Sorting area
1771+
self.move_coords_to_angles = [
1772+
[-61.61, 3.6, -100.63, 12.91, 95.44, -59.06], # D Sorting area
1773+
[-25.22, -43.94, -39.9, 9.22, 90.43, -21.18], # C Sorting area
1774+
[58.18, -42.89, -32.69, -1.31, 89.38, 45.52], # A Sorting area
1775+
[100.1, -0.17, -95.0, 11.77, 97.64, -77.87], # B Sorting area
17791776
]
17801777
self.myCobot.send_coords([x, y, 230, -173.84, -0.14, -74.37], 100, 1)
17811778
time.sleep(3)
@@ -1786,7 +1783,7 @@ def moved(self, x, y, yaw_degrees):
17861783
# open pump
17871784
if func != 'object recognition' or func != '物体识别' or func != 'Color recognition grip' or func != '颜色识别 夹爪' or func != 'Intelligent gripping' or func != '智能夹取':
17881785
self.pump_on()
1789-
self.stop_wait(2)
1786+
time.sleep(1.5)
17901787
tmp = []
17911788
while True:
17921789
if not tmp:
@@ -1795,7 +1792,7 @@ def moved(self, x, y, yaw_degrees):
17951792
break
17961793
self.myCobot.send_angles([tmp[0], -0.71, -54.49, -23.02, 89.56, tmp[5]],
17971794
25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
1798-
time.sleep(3)
1795+
time.sleep(3.5)
17991796
if not self.auto_mode_status:
18001797
self.crawl_status = False
18011798
self.discern_status = False
@@ -1813,8 +1810,8 @@ def moved(self, x, y, yaw_degrees):
18131810
color = 1
18141811
else:
18151812
color = 0
1816-
self.myCobot.send_coords(self.move_coords[color], 100, 1)
1817-
self.stop_wait(6.5)
1813+
self.myCobot.send_angles(self.move_coords_to_angles[color], 25)
1814+
time.sleep(5)
18181815

18191816
if func == 'object recognition' or func == '物体识别' or func == 'Color recognition grip' or func == '颜色识别 夹爪' or func == 'Intelligent gripping' or func == '智能夹取':
18201817
# open gripper
@@ -1823,7 +1820,7 @@ def moved(self, x, y, yaw_degrees):
18231820
# close pump
18241821
self.pump_off()
18251822

1826-
self.stop_wait(6.5)
1823+
time.sleep(3)
18271824
if func == 'object recognition' or func == '物体识别' or func == 'Color recognition grip' or func == '颜色识别 夹爪' or func == 'Intelligent gripping' or func == '智能夹取':
18281825
self.gripper_off()
18291826
time.sleep(1)

0 commit comments

Comments
 (0)