def bridge(Chest):
event_step_bridge = 0
counter_step = 0
while True:
img = undistort_chest(Chest.imgs)
img = img[50:430, 50:400].copy()
img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
cv2.imshow("img", img)
cv2.waitKey(1)
height = img.shape[0]
width = img.shape[1]
blank_img = np.zeros((height, width, 1), np.uint8)
h, s, v = cv2.split(img)
thresh_value, thresh_img = cv2.threshold(s, 110, 255,
cv2.THRESH_BINARY)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
thresh = cv2.morphologyEx(thresh_img, cv2.MORPH_OPEN, kernel)
cv2.imshow("thresh", thresh)
_, contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
length = len(contours)
list1 = []
for i in range(length):
cnt = contours[i]
epsilon = 0.02 * cv2.arcLength(cnt, True)
approx = cv2.approxPolyDP(cnt, 20, True)
cv2.polylines(blank_img, [approx], 1, (255, 0, 0), 1, cv2.LINE_4)
if event_step_bridge == 0:
if (cv2.isContourConvex(approx)):
list1.append(approx)
flag_hole = True
else:
action_append("Forwalk01")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
else:
list1.append(approx)
flag_hole = True
if flag_hole == True:
area_max_contour, contour_area_max = getAreaMaxContour1(list1)
M = cv2.moments(area_max_contour)
cX = int(M["m10"] / (M["m00"] + 0.0001))
cY = int(M["m01"] / (M["m00"] + 0.0001))
midpoint = [cX, cY]
cv2.polylines(blank_img, [area_max_contour], 1, (255, 0, 255), 1,
cv2.LINE_4)
cv2.circle(blank_img, (cX, cY), 6, (255, 255, 255), -1)
cv2.imshow("blank_img", blank_img)
print(cX, cY)
else:
action_append("Forwalk01")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
cv2.waitKey(1)
if event_step_bridge == 0:
print("event_step_bridge =", event_step_bridge)
x_dis = abs(cX - 150)
print("cX =", cX)
if cX > 150:
if x_dis > 50:
print("Forwalk02")
action_append("Forwalk02")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 30:
print("Forwalk01")
action_append("Forwalk01")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 20:
print("Forwalk00")
action_append("Forwalk00")
while len(action_list) != 0:
time.sleep(0.1)
# else:
# print("Back1Run")
# action_append("Back1Run")
y_dis = abs(cY - 190)
print("cY =", cY)
if cY > 190:
if y_dis > 50:
if cX > 140:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20:
if cX > 300:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cY != 0 and cX <= 170:
event_step_bridge = 1
print("event_step_bridge =", event_step_bridge)
elif cY < 190 and cY != 0:
if y_dis > 50:
if cX > 140:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20:
if cX > 300:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cX <= 170:
event_step_bridge = 1
if cX == 0 and cY == 0:
action_append("Forwalk00")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if event_step_bridge == 1:
print("event_step_bridge =", event_step_bridge)
lines = cv2.HoughLines(blank_img, 1, 3.14159 / 180, 30, None, 0, 0)
cv2.polylines(blank_img, [area_max_contour], 1, (255, 0, 255), 1,
cv2.LINE_4)
k = np.zeros(2, dtype=np.float32)
bb = np.zeros(2, dtype=np.float32)
#cnt=-1
large = 10
small = 10
if lines is not None:
for i in range(0, len(lines)):
rho = lines[i][0][0]
theta = lines[i][0][1]
if (theta > np.pi / 2):
if ((theta - np.pi / 2) < large):
large = theta - np.pi / 2
t_l = np.pi / 2 + large
a = math.cos(t_l)
b = math.sin(t_l)
x0 = a * rho
y0 = b * rho
pt1 = (int(x0 + 1000 * (-b)), int(y0 + 1000 * (a)))
pt2 = (int(x0 - 1000 * (-b)), int(y0 - 1000 * (a)))
k[0] = (pt2[1] - pt1[1]) / (pt2[0] - pt1[0])
bb[0] = float((pt1[1] * pt2[0] - pt2[1] * pt1[0]) /
(pt2[0] - pt1[0]))
else:
if ((np.pi / 2 - theta) < small):
small = np.pi / 2 - theta
t_s = np.pi / 2 - small
a = math.cos(t_s)
b = math.sin(t_s)
x0 = a * rho
y0 = b * rho
pt3 = (int(x0 + 1000 * (-b)), int(y0 + 1000 * (a)))
pt4 = (int(x0 - 1000 * (-b)), int(y0 - 1000 * (a)))
k[1] = (pt4[1] - pt3[1]) / (pt4[0] - pt3[0] +
0.0001)
bb[1] = float((pt3[1] * pt4[0] - pt4[1] * pt3[0]) /
(pt4[0] - pt3[0] + 0.0001))
cv2.line(img, pt1, pt2, (0, 255, 0), 1)
cv2.line(img, pt3, pt4, (0, 255, 0), 1)
cv2.imshow("frame", img)
#cv2.waitKey(1)
left_x_start = k[0] * 590 + bb[0] + 50
left_x_end = k[0] * 50 + bb[0] + 50
left_line = [left_x_end, left_x_start]
right_x_start = k[1] * 590 + bb[1] + 50
right_x_end = k[1] * 50 + bb[1] + 50
right_line = [right_x_end, right_x_start]
dx_l, deg_l, posi_left = Calculate_position_bridge(left_line)
print('left', dx_l, deg_l)
dx_r, deg_r, posi_right = Calculate_position_bridge(right_line)
print('right', dx_r, deg_r)
if posi_left == False and posi_right == False:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
elif posi_left == True and posi_right == True:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if dx_l <= 8 or dx_r >= 12:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif dx_r <= 8 or dx_l >= 12:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
deg_avg = (deg_l + deg_r) / 2
if deg_avg > 5:
print("turn000L")
action_append("turn000L")
while len(action_list) != 0:
time.sleep(0.1)
elif deg_avg < -5:
print("turn000R")
action_append("turn000R")
while len(action_list) != 0:
time.sleep(0.1)
else:
event_step_bridge = 2
if event_step_bridge == 2:
print("event_step_bridge =", event_step_bridge)
blank_img = np.rot90(blank_img).copy()
#lines = cv2.HoughLines(blank_img,1,3.14159/180,30,None,0,0)
lines = cv2.HoughLinesP(blank_img,
1.0,
np.pi / 180,
100,
minLineLength=10,
maxLineGap=15)
if lines is None:
action_append("Forwalk01")
continue
final_image, Final_line, good = group_lines_and_draw_bridge(
blank_img, lines, 'Right')
if Final_line is None:
action_append("Forwalk01")
continue
# cv2.imshow("final_image", final_image)
# cv2.waitKey(1)
dx_line, deg_line, posi = Calculate_position_bridge(Final_line)
print(dx_line, deg_line)
if posi == True:
if dx_line <= 8:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif dx_line >= 12:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if dx_line <= 8:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
elif dx_line >= 12:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
if deg_line > 5:
for i in range(int(deg_line / 5) + 1):
print("turn000L")
action_append("turn000L")
while len(action_list) != 0:
time.sleep(0.1)
elif deg_line < -5:
for i in range(int(abs(deg_line) / 5) + 1):
print("turn000R")
action_append("turn000R")
while len(action_list) != 0:
time.sleep(0.1)
if dx_line >= 8 and dx_line <= 10:
if counter_step > 6:
action_append("fastForward04")
while len(action_list) != 0:
time.sleep(0.1)
event_step_bridge = 3
else:
print("Forwalk02")
action_append("Forwalk02")
while len(action_list) != 0:
time.sleep(0.1)
counter_step = counter_step + 1
if event_step_bridge == 3:
print('bridge finish')
break
def bridge(Chest):
event_step_bridge = 0
while True:
img = undistort_chest(Chest)
img = img[50:430, 50:400].copy()
img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
height = img.shape[0]
width = img.shape[1]
blank_img = np.zeros((height, width, 1), np.uint8)
h, s, v = cv2.split(img)
cv2.imshow("s", s)
thresh_value, thresh_img = cv2.threshold(s, 110, 255,
cv2.THRESH_BINARY)
canny = cv2.Canny(thresh_img, 50, 150)
cv2.imshow("c", canny)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
thresh = cv2.morphologyEx(thresh_img, cv2.MORPH_OPEN, kernel)
#cv2.imshow("thresh", thresh)
contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
length = len(contours)
list1 = []
for i in range(length):
cnt = contours[i]
epsilon = 0.02 * cv2.arcLength(cnt, True)
approx = cv2.approxPolyDP(cnt, 20, True)
#if(len(approx) == 4 or len(approx) == 5):
list1.append(approx)
area_max_contour, contour_area_max = getAreaMaxContour1(list1)
M = cv2.moments(area_max_contour)
cX = int(M["m10"] / (M["m00"] + 0.0001))
cY = int(M["m01"] / (M["m00"] + 0.0001))
cv2.polylines(blank_img, [area_max_contour], 1, (255, 0, 0), 1,
cv2.LINE_4)
# final_image, Final_line, good = group_lines_and_draw(
# blank_img, Lines, wich_side)
midpoint = [cX, cY]
# cv2.polylines(blank_img, [area_max_contour], 1, (255, 0, 255), 1, cv2.LINE_4)
cv2.circle(blank_img, (cX, cY), 6, (255, 255, 255), -1)
cv2.imshow("img", img)
cv2.imshow('thresh_img', thresh_img)
cv2.imshow('thresh', thresh)
cv2.imshow("blank_img", blank_img)
Lines = cv2.HoughLinesP(blank_img,
1.0,
np.pi / 180,
100,
minLineLength=10,
maxLineGap=15)
final_image = draw_lines(blank_img,
Lines,
color=[0, 255, 255],
thickness=4) #for test
final_image, Final_line, good = group_lines_and_draw(
blank_img, Lines, 'Right')
cv2.imshow("origine line", final_image)
print(cX, cY)
cv2.waitKey(0)
def Back_to_center(Chest_img, wich_side='Left'):
"""
split left and right to calculate
return list = Step,Trun,YouJinShen,Right,Clockwise
"""
Filter_length = 130
iteration = 0
while True:
if len(action_list) == 0:
print('Filter_length', Filter_length)
Chest = np.rot90(undistort_chest(Chest_img.imgs)).copy()
cv2.imshow("undistort_chest", Chest)
cv2.waitKey(1)
# continue
if wich_side == 'Right':
ROI_image = Chest[250:550, 240:450] #右侧边缘，胸部
elif wich_side == 'Left':
ROI_image = Chest[250:550, 30:239] #左侧边缘，胸部
# 机器人脚的位置
# ROI_image[340,:] = 255
cv2.imshow("Chest_img", ROI_image)
cv2.waitKey(1)
ROI_image = cv2.pyrMeanShiftFiltering(ROI_image, 9, 25)
cv2.imshow("pyrMeanShiftFiltering", ROI_image)
cv2.waitKey(1)
Canny_img = cv2.Canny(ROI_image, 15, 150)
# cv2.imshow("Canny_img",Canny_img)
# cv2.waitKey(1)
#膨胀加粗边缘
dilate = cv2.dilate(Canny_img,
np.ones((2, 2), np.uint8),
iterations=1)
cv2.imshow("dilate", dilate)
cv2.waitKey(1)
Lines = cv2.HoughLinesP(dilate,
1.0,
np.pi / 180,
100,
minLineLength=Filter_length,
maxLineGap=15)
# final_image = draw_lines(ROI_image,Lines,color=[0,255,0],thickness=2) #for test
# cv2.imshow("origine line",final_image)
# cv2.waitKey(1)
final_image, Final_line, good = group_lines_and_draw(
ROI_image, Lines, wich_side)
if Final_line is None:
if Filter_length > 80:
Filter_length -= 10
else:
iteration += 1
continue
if iteration == 3:
print('No lines for long, just go')
break
cv2.imshow("image line", final_image)
cv2.waitKey(1)
# print('test')
if good:
if wich_side == 'Right':
Final_line[0] = Final_line[0] + 240
Final_line[1] = Final_line[1] + 240
if wich_side == 'Left':
Final_line[0] = Final_line[0] + 30
Final_line[1] = Final_line[1] + 30
dX, deg = Calculate_position(Final_line)
# print('line info',dX,deg)
Step, Trun, Move_action, Turn_action = Move_dicision(
dX, deg, wich_side)
if Step == 0 and Trun == 0:
print('In the center')
break
else:
Step, Trun, Move_action, Turn_action = 0, 0, True, True
print('啥也没看见朋友！')
for i in range(int(Trun)):
action_append(Turn_action)
time.sleep(0.5)
for i in range(int(Step)):
action_append(Move_action)
time.sleep(0.5)
def holeb(Chest):
counter_forwalk = 0
event_step = 0
while True:
if event_step == 0:
frame = undistort_chest(Chest.imgs)
img = frame[30:450, 50:400]
img_test = img.copy()
fsrc = np.array(img, dtype=np.float32) / 255.0
(b, g, r) = cv2.split(fsrc)
#lv
#gray = 2 * g - b - r
gray = 2*b - g- r
(minVal, maxVal, minLoc, maxLoc) = cv2.minMaxLoc(gray)
hist = cv2.calcHist([gray], [0], None, [256], [minVal, maxVal])
gray_u8 = np.array((gray - minVal) / (maxVal - minVal) * 255, dtype=np.uint8)
(t, thresh_img) = cv2.threshold(gray_u8, -1.0, 255, cv2.THRESH_OTSU+cv2.THRESH_BINARY_INV)
cv2.imshow("th", thresh_img)
cv2.imshow("th", thresh_img)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (15, 15))
thresh = cv2.morphologyEx(thresh_img, cv2.MORPH_OPEN, kernel)
# edges = cv2.Canny(thresh,20,100,apertureSize = 3,L2gradient=0)
_, contours, _ = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(img, contours, -1, (0, 255, 0), 2)
#cv2.imshow('c',img)
#cv2.waitKey(1)
length = len(contours)
list1 = []
flag_hole = False
for i in range(length):
cnt = contours[i]
#epsilon = 0.02*cv2.arcLength(cnt,True)
approx = cv2.approxPolyDP(cnt, 15, True)
if (({len(approx) == 4 or len(approx) == 5})
and (cv2.isContourConvex(approx))):
list1.append(approx)
flag_hole = True
if flag_hole == True:
area_max_contour, _ = getAreaMaxContour1(list1)
M = cv2.moments(area_max_contour)
cX = int(M["m10"] / (M["m00"] + 0.0001))
cY = int(M["m01"] / (M["m00"] + 0.0001))
# midpoint=[cX,cY]
print("cX is {} and cY is {}".format(cX, cY))
cv2.circle(img_test, (cX, cY), 6, 1, -1)
cv2.drawContours(img_test, area_max_contour, -1, (0, 0, 255),
2)
cv2.imshow("hole test", img_test)
cv2.waitKey(1)
x_dis = abs(cX - 150)
y_dis = abs(cY - 190)
# y_right = abs(300 - cY)
# y_left = abs(220 - cY)
print('x_dis =', x_dis)
if cX > 150:
if x_dis > 50:
action_append("Forwalk02")
action_append("turn000R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 30:
action_append("Forwalk01")
action_append("turn000R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 20:
action_append("Forwalk00")
action_append("turn000R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
else:
event_step = 1
elif cX > 130 and cX < 170:
event_step = 1
else:
action_append("Back2Run")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if cY > 190:
if y_dis > 50:
if cX > 140:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20:
if cX > 300:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cY != 0 and cX <= 170:
event_step_bridge = 1
print("event_step_bridge =", event_step_bridge)
elif cY < 190:
if y_dis > 50 and cY != 0:
if cX > 140:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20 and cY != 0:
if cX > 300:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cY != 0 and cX <= 170:
event_step_bridge = 1
# cv2.circle(img_test, (cX, cY), 6, 1, -1)
# cv2.drawContours(img_test, area_max_contour, -1, (0, 0, 255), 2)
# cv2.imshow("hole test", img_test)
# cv2.waitKey(1)
else:
action_append("Right02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if event_step == 1:
action_append("turn001R")
action_append("turn001R")
action_append("Right3move")
action_append("Right3move")
action_append("Right3move")
#action_append("Right3move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
event_step = 2
if event_step == 2:
ROI_image = np.rot90(Chest.imgs).copy()[250:550, :]
cv2.imshow("Chest_img", ROI_image)
cv2.waitKey(1)
Canny_img = cv2.Canny(ROI_image, 15, 150)
# cv2.imshow("Canny_img",Canny_img)
# cv2.waitKey(1)
#膨胀加粗边缘
dilate = cv2.dilate(Canny_img,
np.ones((2, 2), np.uint8),
iterations=1)
cv2.imshow("dilate", dilate)
cv2.waitKey(1)
print('event2')
Lines = cv2.HoughLinesP(dilate,
1.0,
np.pi / 180,
100,
minLineLength=50,
maxLineGap=10)
final_image, right_side, _ = group_lines_and_draw(
ROI_image, Lines, 'Right')
if right_side is None:
action_append("Forwalk01")
action_append("turn001R")
continue
dx, deg = Calculate_position(right_side)
print('right', dx, deg)
if dx > 10:
# print("Right3move")
action_append("Right3move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
event_step = 3
else:
event_step = 3
if event_step == 3:
ROI_image = np.rot90(Chest.imgs).copy()[250:550, :]
cv2.imshow("Chest_img", ROI_image)
#cv2.waitKey(1)
Canny_img = cv2.Canny(ROI_image, 15, 150)
# cv2.imshow("Canny_img",Canny_img)
# cv2.waitKey(1)
#膨胀加粗边缘
dilate = cv2.dilate(Canny_img,
np.ones((2, 2), np.uint8),
iterations=1)
#cv2.imshow("dilate",dilate)
#cv2.waitKey(1)
print('event3')
Lines = cv2.HoughLinesP(dilate,
1.0,
np.pi / 180,
100,
minLineLength=50,
maxLineGap=10)
final_image, right_side, _ = group_lines_and_draw(
ROI_image, Lines, 'Right')
if right_side is None:
action_append("Forwalk01")
action_append("turn001R")
continue
dx, deg = Calculate_position(right_side)
print('right', dx, deg)
cv2.imshow("image line", final_image)
cv2.waitKey(1)
if deg >= 7:
print("turn001L")
action_append("turn001L")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif deg <= -7:
print("turn001R")
action_append("turn001R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if dx >= 12:
print("Right02move")
action_append("Right02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif dx <= 8:
print("Left02move")
action_append("Left02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if -7 < deg < 7 and 8 < dx < 12:
print("go straight")
action_append("Forwalk02")
action_append("turn000R")
action_append("Forwalk02")
action_append("turn000R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
counter_forwalk = counter_forwalk + 1
if counter_forwalk == 2:
event_step = 4
if event_step == 4:
ROI_image = np.rot90(Chest.imgs).copy()[250:550, :]
cv2.imshow("Chest_img", ROI_image)
cv2.waitKey(1)
Canny_img = cv2.Canny(ROI_image, 15, 150)
# cv2.imshow("Canny_img",Canny_img)
# cv2.waitKey(1)
#膨胀加粗边缘
dilate = cv2.dilate(Canny_img,
np.ones((2, 2), np.uint8),
iterations=1)
#cv2.imshow("dilate",dilate)
cv2.waitKey(1)
print('event_step == 4')
Lines = cv2.HoughLinesP(dilate,
1.0,
np.pi / 180,
100,
minLineLength=50,
maxLineGap=10)
final_image, right_side, _ = group_lines_and_draw(
ROI_image, Lines, 'Right')
if right_side is None:
action_append("Forwalk01")
action_append("turn001R")
continue
dx, deg = Calculate_position(right_side)
print('right', dx, deg)
cv2.imshow("image line", final_image)
#cv2.waitKey(1)
if deg >= 7:
print("turn001L")
action_append("turn001L")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif deg <= -7:
print("turn001R")
action_append("turn001R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if dx >= 12:
print("Right02move")
action_append("Right02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif dx <= 8:
print("Left02move")
action_append("Left02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if -7 < deg < 7 and 8 < dx < 12:
print("go left")
action_append("Left3move")
action_append("Left3move")
action_append("Left3move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
event_step = 5
if event_step == 5:
print('Finish Hole')
cv2.destroyAllWindows()
break