def findGood():
global currentx, currenty #设为全局变量，存放当前位置
i = 0
# camera = Camera_init() #摄像头初始化
# camera.cap = cap = cv2.VideoCapture(0)
list_goods = [(4, 2), (7, 4), (5, 7), (2, 5)] #货物坐标
for good in list_goods:
i += 1
print("移动进程")
print("机械臂移动过程") #机械臂旋转，参数为整型
armControl.good_arm(i)
runControl.runControl(currentx, currenty, good[0], good[1]) #前往位置
print("机械臂移动过程") #机械臂旋转，参数为整型
armControl.good_arm(i)
time.sleep(1)
path = 'test/image_goods/' + str(i) #测试文件放置
# path = 'image/image_goods/'+str(i)
# print(path)
print("货物拍照进程：")
picture.append(camera.use_Camera(path)) #拍摄照片，传回照片
currentx = good[0]
currenty = good[1] #更新当前路径
print("curx = " + str(currentx) + " cury = " + str(currenty))
time.sleep(1)
pic_good = camera.split_Camera1(picture) #图片切割，获得切割后的图片
def grabPlace(planningSite):
print("root_path:")
print(planningSite)
print(type(planningSite))
global currentx
global currenty
i = 0
for index in planningSite:
if i % 2 == 0: #抓取货物
runControl.runControl(currentx, currenty, index[0],
index[1]) #到指定点
print("货物抓取步骤")
if index[0] == 4:
dir = 4
elif index[0] == 7:
dir = 1
elif index[0] == 5:
dir = 2
elif index[0] == 2:
dir = 3
#机械臂动作和行进是同步进行的，需要进行串口通信：arduino发ok给树莓派
print("机械臂准备工作")
print("nano1")
armControl.good_arm(dir) #机械臂抓取的准备工作——转向
time.sleep(1) #动作完成后延时1s
print("nano2")
armControl.gesture() #准备工作——准备抓取动作
print("nano1")
armControl.paw('m') #爪子张开动作
if index[2].split('-')[1] == '1': #根据需要向左或者右，走半步
print("向左走半步")
halfPath.left(index[0])
elif index[2].split('-')[1] == '3':
print("向右走半步")
halfPath.right(index[0])
time.sleep(1) #左右半步走完之后，延时1s
print("向前走半步")
halfPath.forward(index[0]) #走半步
time.sleep(1) #走半步之后，延时1s
print("nano2")
armControl.gooddown() #爪子下降
iden = good_identify(index[2].split('-')[0])
print("抓取货物动作")
print("nano1")
armControl.grab_good(iden) #机械臂根据不同的物体，做不同的动作，传输的是物品对应的编号
print("nano2")
armControl.init()
print("向后退半步")
halfPath.back(index[0]) #退半步
time.sleep(1) #退半步之后，延时1s
if index[2].split('-')[1] == '1': #根据需要向左或者右，退半步
print("向右退半步")
halfPath.right(index[0])
elif index[2].split('-')[1] == '3':
print("向左退半步")
halfPath.left(index[0])
time.sleep(1) #左右半步退完之后，延时1s
currentx = index[0] #更新当前路径
currenty = index[1]
else: #放置货物
if index[0] >= 0 and index[0] <= 5 and index[1] == 0:
dir = 'd' #放到A货仓
x = index[0]
y = index[1] + 1
elif index[0] == 9 and index[1] >= 0 and index[1] <= 5:
dir = 'a' #放到B货仓
x = index[0] - 1
y = index[1]
elif index[0] >= 4 and index[0] <= 9 and index[1] == 9:
dir = 'b' #放到C货仓
x = index[0]
y = index[1] - 1
elif index[0] == 0 and index[1] >= 4 and index[1] <= 9:
dir = 'c' #放到D货仓
x = index[0] + 1
y = index[1]
else:
continue
runControl.runControl(currentx, currenty, x, y) #到指定点
print("货物放置动作")
print("机械臂准备工作")
print("nano1")
armControl.ware_arm(dir) #机械臂准备工作——转向
if index[2] == 'H':
dir = 'h'
elif index[2] == 'L':
dir = 'j'
print("nano2")
armControl.place_ware(dir) #上下层准备
time.sleep(1)
print("进一步动作")
runControl.runControl(x, y, index[0], index[1])
print("nano2")
armControl.waredown(dir) #货物放下
print("nano1")
armControl.paw('m') #放置
runControl.runControl(index[0], index[1], x, y) #退回一格
# if index[2] == 'H':
# dir = '5'
# elif index[2] == 'L':
# dir = '6'
print("nano1")
armControl.paw('0') #爪子闭合
print("nano2")
armControl.init() #恢复到初始化动作
currentx = x #更新路径
currenty = y
i += 1 #区分抓和放