-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain2.py
194 lines (166 loc) · 6.85 KB
/
main2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
import numpy as np
import quad_detector
import step
import cv2
import electromagnets
import sys
import time
def move(now_x, now_y, dist_x, dist_y):
"""
从(now_x, now_y) -> (dist_x, dist_y)
"""
step.setup()
abs_x = abs(now_x - dist_x) * 50
abs_y = abs(now_y - dist_y) * 50
if abs_x <= 5 and abs_y <= 5:
step.y_backward(3000)
print('sucessfully!')
return True
if dist_x > now_x:
step.x_forward(abs_x)
else: step.x_backward(abs_x)
if dist_y > now_y:
step.y_backward(abs_y)
else: step.y_forward(abs_y)
return False
if __name__ == "__main__":
# 打开摄像头
cap = cv2.VideoCapture(0)
quad_detector = quad_detector.QuadDetector(9999, 200, 200/600, 30, 6) # 初始化视觉类
point_axis = []
black_list = [] # 存黑色棋子
white_list = [] # 存白色棋子
white_cnt, black_cnt = 0, 0
point_key = {} # 用字典存九宫格的坐标
ox, oy = 0, 0
# 回到初始位置
step = step.StepControl() # 初始化步进电机
#############裁剪部分###################
up, down, l, r = 100, -80, 140, -120
###########根据摄像头进行更改############
while True:
e = electromagnets.Electromagnets() # 初始化电磁铁
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
frame = frame[up:down, l:r]
while len(point_axis) < 9:
try:
if len(point_axis) == 0:
# 四边形检测结果
vertices, scale_vertices, intersection = quad_detector.detect(frame) # 检测四边形方框
img_detected = quad_detector.draw(frame) # 绘制检测结果
# 显示摄像头图像，其中的video为窗口名称，frame为图像
cv2.imshow('detect', img_detected)
point_axis = quad_detector.point_list
point_key = quad_detector.point_key
except Exception as e:
print(e)
while len(black_list) < 2:
quad_detector.chess_detection(frame)
black_list = quad_detector.black_list
while len(white_list) < 2:
quad_detector.chess_detection(frame)
white_list = quad_detector.white_list
print(point_key) # 打印出坐标信息
machine_x, machine_y = quad_detector.detectMachineArm(frame)
print('axis:', (machine_x, machine_y)) # 打印出机械臂坐标
# 从机械臂当前位置移动到黑色棋子
while len(black_list) > 0 and len(point_axis) != 0:
if black_cnt >= 2: break
zip_axis = black_list.pop()
black_x, black_y = zip_axis[0], zip_axis[1]
print('white_axis: ', (black_x, black_y))
while True:
e = electromagnets.Electromagnets()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
frame = frame[up:down, l:r]
machine_x, machine_y = quad_detector.detectMachineArm(frame)
if move(machine_x, machine_y, black_x, black_y):
step.down() # 落下电磁铁
time.sleep(0.02)
e.open() # 启动电磁铁吸取
print('拿棋子成功')
time.sleep(0.02)
step.up() # 抬起电磁铁
time.sleep(0.02)
break
# 从黑色棋子移动到指定号九宫格
pos = int(input('please input postion: '))
while True:
e = electromagnets.Electromagnets()
e.open()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
frame = frame[up:down, l:r]
machine_x, machine_y = quad_detector.detectMachineArm(frame)
if move(machine_x, machine_y, point_key[pos][0], point_key[pos][1]):
step.down() # 落下电磁铁
time.sleep(0.02)
e.close() # 释放吸力
time.sleep(0.02)
print('放棋子成功')
step.up() # 抬起电磁铁
time.sleep(0.02)
black_cnt += 1
break
# 从机械臂当前位置移动到白色棋子
while len(white_list) > 0 and len(point_axis) != 0:
if white_cnt >= 2: break
zip_axis = white_list.pop()
white_x, white_y = zip_axis[0], zip_axis[1]
print('white_axis: ', (white_x, white_y))
while True:
e = electromagnets.Electromagnets()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
frame = frame[up:down, l:r]
machine_x, machine_y = quad_detector.detectMachineArm(frame)
if move(machine_x, machine_y, white_x, white_y):
step.down() # 落下电磁铁
time.sleep(0.02)
e.open() # 启动电磁铁吸取
print('拿棋子成功')
time.sleep(0.02)
step.up() # 抬起电磁铁
time.sleep(0.02)
break
# 从白色棋子移动到指定号九宫格
pos = int(input('please input postion: '))
while True:
e = electromagnets.Electromagnets()
e.open()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
hx, frame = cap.read()
frame = frame[up:down, l:r]
machine_x, machine_y = quad_detector.detectMachineArm(frame)
if move(machine_x, machine_y, point_key[pos][0], point_key[pos][1]):
step.down() # 落下电磁铁
time.sleep(0.02)
e.close() # 释放吸力
time.sleep(0.02)
print('放棋子成功')
step.up() # 抬起电磁铁
time.sleep(0.02)
white_cnt += 1
break