You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
2558 lines
52 KiB
2558 lines
52 KiB
from fastapi import APIRouter
|
|
from starlette.requests import Request
|
|
from tortoise.queryset import QuerySet, Prefetch
|
|
from models.agv import (
|
|
Agv
|
|
)
|
|
from models.warehouse import (
|
|
Hole,
|
|
Vacancy
|
|
)
|
|
from helper import respond_to
|
|
from tortoise.contrib.pydantic import pydantic_model_creator
|
|
import requests
|
|
from typing import List
|
|
|
|
import threading
|
|
|
|
from plc.tools import (
|
|
bytes_to_int_list,
|
|
float2intList,
|
|
intList2Float
|
|
)
|
|
import struct
|
|
import time
|
|
import math
|
|
from pydantic import BaseModel
|
|
from typing import Optional
|
|
|
|
from conf import setting
|
|
|
|
from cam.getParam import getParam
|
|
from cam.axis import (
|
|
labelMapBigDisk,
|
|
labelMapLittleDisk,
|
|
labelMapLittleBox,
|
|
labelMapBigBox
|
|
)
|
|
|
|
|
|
from plc.cmd import(
|
|
cmd_type,
|
|
setSubPaths,
|
|
pass_door,
|
|
setLayer,
|
|
moveArm,
|
|
taskNum,
|
|
setTasks,
|
|
clear_batch,
|
|
readStatus,
|
|
write_batch,
|
|
write_batch_float,
|
|
#Cam related
|
|
readCamStatus,
|
|
setCamIntList,
|
|
camDone,
|
|
doorReady,
|
|
write_task_batch
|
|
)
|
|
|
|
from algorithm.genpath import (
|
|
genPath,
|
|
getAgvStop
|
|
)
|
|
|
|
import traceback
|
|
|
|
#second capture count
|
|
CNT = 0
|
|
LASTITEM = None
|
|
LASTX = None
|
|
LASTY = None
|
|
|
|
LABELMAP = []
|
|
|
|
class OrderM(BaseModel):
|
|
ordertype: int
|
|
layer: int
|
|
stop: str
|
|
|
|
|
|
class FullOrder(BaseModel):
|
|
ordertype_start: int
|
|
layer_start: int
|
|
stop_start: str
|
|
|
|
ordertype_end: int
|
|
layer_end: int
|
|
stop_end: str
|
|
|
|
agv_router = APIRouter(prefix='/agv')
|
|
|
|
|
|
ip = "http://127.0.0.1:8000"
|
|
endpoint_getassgin = "{}/api/scheduler/assign/".format(ip)
|
|
endpoint_done = "{}/api/scheduler/assign/done/".format(ip)
|
|
endpoint_failed = "{}/api/scheduler/assign/failed/".format(ip)
|
|
|
|
|
|
@agv_router.get("/charge", summary='充电测试')
|
|
async def charge(request: Request):
|
|
|
|
clear_batch()
|
|
|
|
orderType = 4
|
|
layer = 0
|
|
end = "01-4002-0"
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderType
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layer
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
|
|
@agv_router.get("/test")
|
|
async def test():
|
|
|
|
vl_output = []
|
|
|
|
for i in range(100):
|
|
vl_output.append(0)
|
|
|
|
#命令类型
|
|
vl_output[0] = 1
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = 2
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
vl_output[10] = 1
|
|
vl_output[11] = 110
|
|
vl_output[12] = 111
|
|
vl_output[13] = 112
|
|
vl_output[14] = 113
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
vl_output = []
|
|
|
|
for i in range(100):
|
|
vl_output.append(0)
|
|
|
|
#命令类型
|
|
vl_output[0] = 2
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = 2
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# 311-2
|
|
vl_output[10] = 113
|
|
vl_output[11] = 112
|
|
vl_output[12] = 111
|
|
vl_output[13] = 110
|
|
vl_output[14] = 1
|
|
vl_output[15] = 2
|
|
vl_output[16] = 310
|
|
vl_output[17] = 311
|
|
|
|
# vl_output[10] = 1
|
|
# vl_output[11] = 110
|
|
# vl_output[12] = 111
|
|
# vl_output[13] = 112
|
|
|
|
print("T2: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
@agv_router.get("/test2")
|
|
async def test2():
|
|
|
|
vl_output = []
|
|
|
|
for i in range(100):
|
|
vl_output.append(0)
|
|
|
|
vl_output[0] = 1
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = 2
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
vl_output[10] = 311
|
|
# vl_output[11] = 110
|
|
# vl_output[12] = 111
|
|
# vl_output[13] = 112
|
|
# vl_output[14] = 113
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
vl_output = []
|
|
|
|
for i in range(100):
|
|
vl_output.append(0)
|
|
|
|
#命令类型
|
|
vl_output[0] = 2
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = 5
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
vl_output[10] = 311
|
|
vl_output[11] = 310
|
|
vl_output[12] = 2
|
|
vl_output[13] = 1
|
|
vl_output[14] = 110
|
|
vl_output[15] = 111
|
|
|
|
print("T2: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
|
|
@agv_router.post("/test3", summary='整单测试')
|
|
async def test3(request: Request, orderm: OrderM):
|
|
|
|
clear_batch()
|
|
|
|
orderType = orderm.ordertype
|
|
layer = orderm.layer
|
|
end = orderm.stop
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderType
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layer
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
|
|
|
|
@agv_router.post("/test4", summary='一轮测试')
|
|
async def test4(request: Request, fullOrder: FullOrder):
|
|
|
|
|
|
orderType = fullOrder.ordertype_start
|
|
layer = fullOrder.layer_start
|
|
end = fullOrder.stop_start
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderType
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layer
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
#second round
|
|
|
|
orderType = fullOrder.ordertype_end
|
|
layer = fullOrder.layer_end
|
|
end = fullOrder.stop_end
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderType
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layer
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
@agv_router.post("/testSendTask", summary='testSendTask')
|
|
async def test4(request: Request):
|
|
|
|
task = {'01-0113-3': {'orderid': 'tr43frr', 'name': '试剂3', 'typetask': 3, 'action': 5, 'quantity': 1, 'coordinates': '01-0111-3', 'traynum': '1R07'}}
|
|
sendTask(task)
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
@agv_router.get("/write")
|
|
async def write():
|
|
# write_batch([1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20])
|
|
|
|
drawer = 1.3
|
|
door = 2.5
|
|
|
|
d_drawer = struct.pack('>f', float(drawer))
|
|
d_door = struct.pack('>f', float(door))
|
|
|
|
output = d_drawer+d_door
|
|
|
|
vl_output = bytes_to_int_list(output)
|
|
# print("vl_output: ", vl_output)
|
|
|
|
# # vl_output = [3,4,5,6]
|
|
|
|
# write_batch(vl_output)
|
|
|
|
# vl_output = [3.1]
|
|
# vl_output = [3.1, 5.4, 6.7]
|
|
|
|
write_batch_float(vl_output)
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
'''
|
|
Label
|
|
'01-0115-3-5'
|
|
'''
|
|
def getEndStopLayer(label):
|
|
parts = label.split('-')
|
|
if len(parts) > 2:
|
|
return "{}-{}".format(parts[0], parts[1]), int(parts[2])
|
|
elif len(parts) > 2:
|
|
return "{}-{}".format(parts[0], parts[1]), 0
|
|
elif len(parts) > 1:
|
|
return "{}-0000".format(parts[0]), 0
|
|
else:
|
|
raise Exception("Wrong Label")
|
|
|
|
|
|
def checkPassDoor(label):
|
|
parts = label.split('-')
|
|
|
|
inDoorStop = ["08", "09", "10", "11"]
|
|
|
|
#if check pass door, return True
|
|
for stop in inDoorStop:
|
|
if stop == parts[0]:
|
|
# pass
|
|
return 1
|
|
|
|
#If not return, return False
|
|
# not pass
|
|
return 2
|
|
|
|
|
|
def checkMoveArm(label):
|
|
parts = label.split('-')
|
|
|
|
if len(parts) > 3:
|
|
# Move Arm
|
|
return 1
|
|
else:
|
|
# Don't Move Arm
|
|
return 2
|
|
|
|
|
|
'''
|
|
Get one Box, then put it where it belongs
|
|
'''
|
|
|
|
def sendTask(task):
|
|
|
|
destLabel = list(task.keys())[0]
|
|
|
|
if 'Return' == destLabel:
|
|
print("Return Task!")
|
|
return
|
|
|
|
taskBody = task[destLabel]
|
|
|
|
srcLabel = taskBody["coordinates"]
|
|
|
|
#get
|
|
orderTypeSrc = 1
|
|
#
|
|
endSrc, layerSrc = getEndStopLayer(srcLabel)
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderTypeSrc
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = checkPassDoor(endSrc)
|
|
|
|
#取框层数
|
|
vl_output[2] = layerSrc
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = checkMoveArm(srcLabel)
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, endSrc)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
#second round
|
|
|
|
#get
|
|
orderTypeDest = 2
|
|
#
|
|
endDest, layerDest = getEndStopLayer(destLabel)
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderTypeDest
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = checkPassDoor(endDest)
|
|
|
|
#取框层数
|
|
vl_output[2] = layerDest
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = checkMoveArm(destLabel)
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, endDest)
|
|
print("Path2: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T2: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
|
|
|
|
def returnBox(startStop, layerSrc):
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = 2
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 1
|
|
|
|
#取框层数
|
|
vl_output[2] = layerSrc
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
paths = [startStop]
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("returnBox: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
async def getAvaiableHole(taskBody, label):
|
|
|
|
print("taskBody['typetask']: ", taskBody["typetask"])
|
|
|
|
if 1 == taskBody["typetask"]:
|
|
#Standard A
|
|
vacancy = await Vacancy.get_or_none(number = "Right", is_active = True)
|
|
if vacancy:
|
|
await vacancy.fetch_related("holes")
|
|
holes = await vacancy.holes
|
|
|
|
holesLen = len(holes)
|
|
if holesLen < setting.AGVCAP[taskBody["typetask"]]:
|
|
|
|
hole = await Hole.create(
|
|
number = holesLen + 1,
|
|
traynum = vacancy.number,
|
|
typetask = taskBody["typetask"],
|
|
orderid = taskBody["orderid"],
|
|
coordinates = taskBody["coordinates"],
|
|
coordinatessrc = label
|
|
)
|
|
|
|
await vacancy.holes.add(hole)
|
|
#update state
|
|
|
|
return hole.number + setting.LITDISKDIFF
|
|
|
|
else:
|
|
return -1
|
|
|
|
|
|
elif 3 == taskBody["typetask"]:
|
|
#Standard B
|
|
vacancy = await Vacancy.get_or_none(number = "Left", is_active = True)
|
|
print("vacancy: ", vacancy)
|
|
if vacancy:
|
|
await vacancy.fetch_related("holes")
|
|
holes = await vacancy.holes
|
|
print("holes: ", holes, setting.AGVCAP[taskBody["typetask"]])
|
|
holesLen = len(holes)
|
|
if holesLen < setting.AGVCAP[taskBody["typetask"]]:
|
|
|
|
hole = await Hole.create(
|
|
number = holesLen + 1,
|
|
traynum = vacancy.number,
|
|
typetask = taskBody["typetask"],
|
|
orderid = taskBody["orderid"],
|
|
coordinates = taskBody["coordinates"],
|
|
coordinatessrc = label
|
|
)
|
|
|
|
await vacancy.holes.add(hole)
|
|
#update state
|
|
|
|
return hole.number + setting.BIGDISKDIFF
|
|
|
|
else:
|
|
return -1
|
|
|
|
elif 5 == taskBody["typetask"]:
|
|
pass
|
|
elif 7 == taskBody["typetask"]:
|
|
pass
|
|
elif 9 == taskBody["typetask"]:
|
|
#Non Standard
|
|
pass
|
|
|
|
return -1
|
|
|
|
|
|
async def getHoleDestNumber(destLabel):
|
|
|
|
hole = await Hole.get_or_none(coordinatessrc = destLabel, is_active = True)
|
|
if hole:
|
|
if "Right" == hole.traynum:
|
|
return hole.number + setting.LITDISKDIFF
|
|
elif "Left" == hole.traynum:
|
|
return hole.number + setting.BIGDISKDIFF
|
|
else:
|
|
raise Exception("Cannot match Hole traynum!")
|
|
|
|
|
|
async def getAllTask(tasks = []):
|
|
global CNT
|
|
global LABELMAP
|
|
|
|
# clear all register related in PLC
|
|
clear_batch()
|
|
|
|
CNT = 0
|
|
LABELMAP = []
|
|
|
|
print("getAllTask: ", tasks)
|
|
|
|
task = tasks[0]
|
|
|
|
destLabel = list(task.keys())[0]
|
|
|
|
if 'Return' == destLabel:
|
|
print("Return Task!")
|
|
return
|
|
|
|
taskBody = task[destLabel]
|
|
|
|
srcLabel = taskBody["coordinates"]
|
|
|
|
#get
|
|
orderTypeSrc = 1
|
|
#
|
|
endSrc, layerSrc = getEndStopLayer(srcLabel)
|
|
|
|
tasktype = taskBody["typetask"]
|
|
if 1 == tasktype:
|
|
# little box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapLittleBox
|
|
elif 3 == tasktype:
|
|
# big box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapBigBox
|
|
else:
|
|
print("Cannot Match")
|
|
|
|
print("LABELMAP: ", LABELMAP)
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
# vl_output[0] = orderTypeSrc
|
|
vl_output[0] = 1
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = checkPassDoor(endSrc)
|
|
|
|
#取框层数
|
|
vl_output[2] = layerSrc
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = checkMoveArm(srcLabel)
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = []
|
|
paths = genPath(startStop, endSrc)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
if 1 == checkMoveArm(srcLabel):
|
|
totalTask = len(tasks)
|
|
vl_output = []
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
# total tasks
|
|
vl_output[0] = totalTask
|
|
|
|
idx = 1
|
|
for task in tasks:
|
|
destLabel = list(task.keys())[0]
|
|
|
|
#check AGV坐标系统 for who to get diff
|
|
tasktype = task[destLabel]["typetask"]
|
|
diff = 0
|
|
if 1 == tasktype:
|
|
diff = 300
|
|
elif 3 == tasktype:
|
|
diff = 400
|
|
else:
|
|
print("getAllTask Cannot match taskType: ", tasktype)
|
|
|
|
#AGV Related Operation
|
|
getLabel = int(task[destLabel]["coordinates"].split("-")[-1]) + diff
|
|
putLabel = await getAvaiableHole(task[destLabel], destLabel)
|
|
|
|
if 0 > putLabel:
|
|
raise Exception("Cannot find avaible Hole!")
|
|
|
|
vl_output[idx] = getLabel
|
|
vl_output[idx + 1] = putLabel
|
|
vl_output[idx + 2] = task[destLabel]["height"]
|
|
vl_output[idx + 3] = task[destLabel]["cap"]
|
|
|
|
#update next task index
|
|
idx = idx + 4
|
|
|
|
print("getAllTask Sub Tasks: ", vl_output)
|
|
write_task_batch(vl_output)
|
|
|
|
#clear final task
|
|
clearCmd()
|
|
|
|
#if need to get single bottle,
|
|
# return box after get task finish
|
|
if 1 == checkMoveArm(srcLabel):
|
|
#return box after get
|
|
returnBox(paths[-1], layerSrc)
|
|
clearCmd()
|
|
|
|
|
|
async def putAllTask(tasks = []):
|
|
global CNT
|
|
global LABELMAP
|
|
|
|
# clear all register related in PLC
|
|
clear_batch()
|
|
|
|
CNT = 0
|
|
LABELMAP = []
|
|
|
|
tasks.reverse()
|
|
print("putAllTask: ", tasks)
|
|
|
|
task = tasks[0]
|
|
|
|
destLabel = list(task.keys())[0]
|
|
|
|
if 'Return' == destLabel:
|
|
print("Return Task!")
|
|
return
|
|
|
|
#second round
|
|
#put
|
|
orderTypeDest = 2
|
|
#
|
|
endDest, layerDest = getEndStopLayer(destLabel)
|
|
|
|
taskBody = task[destLabel]
|
|
tasktype = taskBody["typetask"]
|
|
if 1 == tasktype:
|
|
# little box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapLittleBox
|
|
elif 3 == tasktype:
|
|
# big box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapBigBox
|
|
else:
|
|
print("Cannot Match")
|
|
|
|
print("LABELMAP: ", LABELMAP)
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = 2
|
|
|
|
#if return single, first need to get a box
|
|
if 1 == checkMoveArm(destLabel):
|
|
vl_output[0] = 1
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = checkPassDoor(endDest)
|
|
|
|
#取框层数
|
|
vl_output[2] = layerDest
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = checkMoveArm(destLabel)
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, endDest)
|
|
print("Path2: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T2: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
if 1 == checkMoveArm(destLabel):
|
|
totalTask = len(tasks)
|
|
vl_output = []
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
# total tasks
|
|
vl_output[0] = totalTask
|
|
|
|
idx = 1
|
|
for task in tasks:
|
|
destLabel = list(task.keys())[0]
|
|
|
|
#check AGV坐标系统 for who to get diff
|
|
tasktype = task[destLabel]["typetask"]
|
|
diff = 0
|
|
if 1 == tasktype:
|
|
diff = 300
|
|
elif 3 == tasktype:
|
|
diff = 400
|
|
else:
|
|
print("putAllTask Cannot match taskType: ", tasktype)
|
|
|
|
#AGV Related Operation
|
|
getLabel = await getHoleDestNumber(destLabel)
|
|
putLabel = int(destLabel.split("-")[-1]) + diff
|
|
|
|
if 0 > putLabel:
|
|
raise Exception("Cannot find avaible Hole!")
|
|
|
|
vl_output[idx] = getLabel
|
|
vl_output[idx + 1] = putLabel
|
|
vl_output[idx + 2] = task[destLabel]["height"]
|
|
vl_output[idx + 3] = task[destLabel]["cap"]
|
|
|
|
#update next task index
|
|
idx = idx + 4
|
|
|
|
print("putAllTask Sub Tasks: ", vl_output)
|
|
write_task_batch(vl_output)
|
|
|
|
#clear final task
|
|
clearCmd()
|
|
|
|
#if need to get single bottle,
|
|
# return box after get task finish
|
|
if 1 == checkMoveArm(destLabel):
|
|
#return box after get
|
|
returnBox(paths[-1], layerDest)
|
|
clearCmd()
|
|
|
|
|
|
|
|
async def clearHoles():
|
|
holes = await Hole.all().filter(is_active = True)
|
|
for hole in holes:
|
|
hole.is_active = False
|
|
await hole.save()
|
|
|
|
vacancies = await Vacancy.all()
|
|
|
|
for vacany in vacancies:
|
|
await vacany.holes.clear()
|
|
|
|
|
|
@agv_router.get("/getassgin")
|
|
async def getassgin():
|
|
res = requests.get(endpoint_getassgin)
|
|
obj = None
|
|
if res.status_code < 300:
|
|
obj = res.json()
|
|
|
|
try:
|
|
assignment = obj["data"]
|
|
|
|
if assignment:
|
|
# clear previous holes
|
|
await clearHoles()
|
|
|
|
agvs = await Agv.filter(
|
|
agvid = assignment["agvid"],
|
|
is_valid = True,
|
|
is_active = True,
|
|
)
|
|
|
|
agv = None
|
|
if len(agvs) > 0:
|
|
agv = agvs[0]
|
|
agv.curorder = assignment["orderid"]
|
|
agv.curitinerary = assignment["itinerary"]
|
|
await agv.save()
|
|
else:
|
|
agv = await Agv.create(
|
|
agvid = assignment["agvid"],
|
|
curorder = assignment["orderid"],
|
|
curitinerary = assignment["itinerary"]
|
|
)
|
|
|
|
wholeBoxLabel = None
|
|
wholeBoxTask = None
|
|
|
|
taskDic = {}
|
|
#Combine all task in same box
|
|
for task in assignment["cmd"]:
|
|
print("Task: ", task)
|
|
destLabel = list(task.keys())[0]
|
|
|
|
parts = task[destLabel]["coordinates"].split("-")
|
|
|
|
# change label '01-0154-1-1' -> '01-0154-1'
|
|
if len(parts) > 3:
|
|
taskLabel = "-".join(parts[0:3])
|
|
else:
|
|
wholeBoxLabel = "-".join(parts)
|
|
wholeBoxTask = task
|
|
continue
|
|
|
|
try:
|
|
taskDic[taskLabel].append(task)
|
|
except:
|
|
taskDic[taskLabel] = [task]
|
|
|
|
taskDicKeys = list(taskDic.keys())
|
|
taskDicKeys.sort()
|
|
|
|
#Whole get task will put at the end of task list
|
|
if wholeBoxLabel and wholeBoxTask:
|
|
taskDic[wholeBoxLabel] = [wholeBoxTask]
|
|
taskDicKeys.append(wholeBoxLabel)
|
|
|
|
for key in taskDicKeys:
|
|
await getAllTask(taskDic[key])
|
|
|
|
#where to put in AGV plate in getting task !!!
|
|
#where to get from AGV plate in putting task !!!
|
|
|
|
#################
|
|
### put whole task!
|
|
#################
|
|
|
|
wholeBoxLabel = None
|
|
wholeBoxTask = None
|
|
|
|
taskDic = {}
|
|
#Combine all task in same box
|
|
for task in assignment["cmd"]:
|
|
print("Task: ", task)
|
|
destLabel = list(task.keys())[0]
|
|
|
|
parts = destLabel.split("-")
|
|
|
|
# change label '01-0154-1-1' -> '01-0154-1'
|
|
if len(parts) > 3:
|
|
taskLabel = "-".join(parts[0:3])
|
|
else:
|
|
wholeBoxLabel = "-".join(parts)
|
|
wholeBoxTask = task
|
|
continue
|
|
|
|
try:
|
|
taskDic[taskLabel].append(task)
|
|
except:
|
|
taskDic[taskLabel] = [task]
|
|
|
|
taskDicKeys = list(taskDic.keys())
|
|
taskDicKeys.sort()
|
|
|
|
#Whole get task will put at the end of task list
|
|
if wholeBoxLabel and wholeBoxTask:
|
|
taskDic[wholeBoxLabel] = [wholeBoxTask]
|
|
taskDicKeys.insert(0, wholeBoxLabel)
|
|
|
|
for key in taskDicKeys:
|
|
await putAllTask(taskDic[key])
|
|
|
|
await clearHoles()
|
|
|
|
except Exception as e:
|
|
traceback.print_exc()
|
|
print("getassgin: ", e)
|
|
|
|
|
|
return respond_to(code=res.status_code, data=obj)
|
|
|
|
@agv_router.get("/done/{assignment_id}")
|
|
async def get_done(assignment_id: str):
|
|
print("assignment_id: ", endpoint_done + assignment_id)
|
|
res = requests.put(endpoint_done + assignment_id)
|
|
obj = None
|
|
if res.status_code < 300:
|
|
obj = res.json()
|
|
|
|
return respond_to(code=res.status_code, data=obj)
|
|
|
|
#'http://localhost:8000/api/scheduler/assign/done/e56ea677-4dd1-46e6-8e4f-c7f25ff5893a'
|
|
@agv_router.get("/failed/{assignment_id}")
|
|
async def get_failed(assignment_id: str):
|
|
print("assignment_id: ", endpoint_failed + assignment_id)
|
|
res = requests.put(endpoint_failed + assignment_id)
|
|
obj = None
|
|
if res.status_code < 300:
|
|
obj = res.json()
|
|
|
|
return respond_to(code=res.status_code, data=obj)
|
|
|
|
@agv_router.get('/list', summary="列出所有AGV小车的状态")
|
|
async def index(request: Request, page_no: int = 1, page_size: int = 20):
|
|
"""
|
|
列出所有AGV小车的状态
|
|
:param page_no: 1
|
|
:param page_size: 20
|
|
:return:
|
|
"""
|
|
offset = (page_no - 1) * page_size
|
|
|
|
query = QuerySet(Agv).filter(is_valid=True, is_active=True)
|
|
|
|
count = await query.count()
|
|
agvs = await query.limit(page_size).offset(offset)
|
|
|
|
return respond_to(code=200, data=dict(count=count, data=agvs))
|
|
|
|
|
|
|
|
@agv_router.get('/float', summary="浮点测试")
|
|
async def floatTest(request: Request):
|
|
|
|
xStatus = readCamStatus()
|
|
# xStatus = readXStatus()[0]
|
|
|
|
xStatus = list(xStatus)
|
|
|
|
xValue = intList2Float(xStatus[0:2])
|
|
print("xValue: ", xValue)
|
|
|
|
yValue = intList2Float(xStatus[2:4])
|
|
print("yValue: ", yValue)
|
|
|
|
zValue = intList2Float(xStatus[4:6])
|
|
print("zValue: ", zValue)
|
|
|
|
rValue = intList2Float(xStatus[6:8])
|
|
print("rValue: ", rValue)
|
|
|
|
DDXList = float2intList(5.12)
|
|
print("DDXList: ", DDXList)
|
|
|
|
DDYList = float2intList(3.14)
|
|
print("DDYList: ", DDYList)
|
|
|
|
Delta = DDXList + DDYList
|
|
|
|
setCamIntList(Delta)
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
def move1(vl_output):
|
|
# 黑瓶 碘酸钾
|
|
###############
|
|
# backward
|
|
vl_output[0] = 6
|
|
#任务1 get
|
|
vl_output[1] = 102
|
|
#put
|
|
vl_output[2] = 305
|
|
#height
|
|
vl_output[3] = 117
|
|
#diameter
|
|
vl_output[4] = 50
|
|
|
|
#任务2 get
|
|
vl_output[5] = 305
|
|
#put
|
|
vl_output[6] = 205
|
|
#height
|
|
vl_output[7] = 117
|
|
#diameter
|
|
vl_output[8] = 50
|
|
|
|
#任务3 get
|
|
vl_output[9] = 205
|
|
#put
|
|
vl_output[10] = 206
|
|
#height
|
|
vl_output[11] = 117
|
|
#diameter
|
|
vl_output[12] = 50
|
|
|
|
|
|
|
|
###############
|
|
# forward
|
|
#任务1 get
|
|
vl_output[13] = 206
|
|
#put
|
|
vl_output[14] = 205
|
|
#height
|
|
vl_output[15] = 117
|
|
#diameter
|
|
vl_output[16] = 50
|
|
|
|
#任务2 get
|
|
vl_output[17] = 205
|
|
#put
|
|
vl_output[18] = 305
|
|
#height
|
|
vl_output[19] = 117
|
|
#diameter
|
|
vl_output[20] = 50
|
|
|
|
#任务3 get
|
|
vl_output[21] = 305
|
|
#put
|
|
vl_output[22] = 102
|
|
#height
|
|
vl_output[23] = 117
|
|
#diameter
|
|
vl_output[24] = 50
|
|
|
|
return vl_output
|
|
|
|
def move2(vl_output):
|
|
# 红盖 钙X酸
|
|
###############
|
|
# backward
|
|
vl_output[0] = 6
|
|
#任务1 get
|
|
vl_output[1] = 102
|
|
#put
|
|
vl_output[2] = 305
|
|
#height
|
|
vl_output[3] = 90
|
|
#diameter
|
|
vl_output[4] = 50
|
|
|
|
#任务2 get
|
|
vl_output[5] = 305
|
|
#put
|
|
vl_output[6] = 205
|
|
#height
|
|
vl_output[7] = 90
|
|
#diameter
|
|
vl_output[8] = 50
|
|
|
|
#任务3 get
|
|
vl_output[9] = 205
|
|
#put
|
|
vl_output[10] = 206
|
|
#height
|
|
vl_output[11] = 90
|
|
#diameter
|
|
vl_output[12] = 50
|
|
|
|
|
|
|
|
###############
|
|
# forward
|
|
#任务1 get
|
|
vl_output[13] = 206
|
|
#put
|
|
vl_output[14] = 205
|
|
#height
|
|
vl_output[15] = 90
|
|
#diameter
|
|
vl_output[16] = 50
|
|
|
|
#任务2 get
|
|
vl_output[17] = 205
|
|
#put
|
|
vl_output[18] = 305
|
|
#height
|
|
vl_output[19] = 90
|
|
#diameter
|
|
vl_output[20] = 50
|
|
|
|
#任务3 get
|
|
vl_output[21] = 305
|
|
#put
|
|
vl_output[22] = 102
|
|
#height
|
|
vl_output[23] = 90
|
|
#diameter
|
|
vl_output[24] = 50
|
|
|
|
return vl_output
|
|
|
|
def move3(vl_output):
|
|
# 乙醇
|
|
###############
|
|
# backward
|
|
vl_output[0] = 6
|
|
#任务1 get
|
|
vl_output[1] = 102
|
|
#put
|
|
vl_output[2] = 305
|
|
#height
|
|
vl_output[3] = 170
|
|
#diameter
|
|
vl_output[4] = 35
|
|
|
|
#任务2 get
|
|
vl_output[5] = 305
|
|
#put
|
|
vl_output[6] = 205
|
|
#height
|
|
vl_output[7] = 170
|
|
#diameter
|
|
vl_output[8] = 35
|
|
|
|
#任务3 get
|
|
vl_output[9] = 205
|
|
#put
|
|
vl_output[10] = 206
|
|
#height
|
|
vl_output[11] = 170
|
|
#diameter
|
|
vl_output[12] = 35
|
|
|
|
|
|
|
|
###############
|
|
# forward
|
|
#任务1 get
|
|
vl_output[13] = 206
|
|
#put
|
|
vl_output[14] = 205
|
|
#height
|
|
vl_output[15] = 170
|
|
#diameter
|
|
vl_output[16] = 35
|
|
|
|
#任务2 get
|
|
vl_output[17] = 205
|
|
#put
|
|
vl_output[18] = 305
|
|
#height
|
|
vl_output[19] = 170
|
|
#diameter
|
|
vl_output[20] = 35
|
|
|
|
#任务3 get
|
|
vl_output[21] = 305
|
|
#put
|
|
vl_output[22] = 102
|
|
#height
|
|
vl_output[23] = 170
|
|
#diameter
|
|
vl_output[24] = 35
|
|
|
|
return vl_output
|
|
|
|
def move4(vl_output):
|
|
vl_output[0] = 2
|
|
|
|
#任务1 get
|
|
vl_output[1] = 206
|
|
#put
|
|
vl_output[2] = 202
|
|
#height
|
|
vl_output[3] = 117
|
|
#diameter
|
|
vl_output[4] = 50
|
|
|
|
#任务1 get
|
|
vl_output[5] = 202
|
|
#put
|
|
vl_output[6] = 205
|
|
#height
|
|
vl_output[7] = 117
|
|
#diameter
|
|
vl_output[8] = 50
|
|
|
|
return vl_output
|
|
|
|
# 大白瓶
|
|
|
|
# 大白瓶
|
|
def move5(vl_output):
|
|
# 大白瓶
|
|
###############
|
|
# backward
|
|
vl_output[0] = 6
|
|
#任务1 get
|
|
vl_output[1] = 101
|
|
#put
|
|
vl_output[2] = 102
|
|
#height
|
|
vl_output[3] = 180
|
|
#diameter
|
|
vl_output[4] = 40
|
|
|
|
#任务2 get
|
|
vl_output[5] = 102
|
|
#put
|
|
vl_output[6] = 401
|
|
#height
|
|
vl_output[7] = 180
|
|
#diameter
|
|
vl_output[8] = 40
|
|
|
|
#任务3 get
|
|
vl_output[9] = 401
|
|
#put
|
|
vl_output[10] = 403
|
|
#height
|
|
vl_output[11] = 180
|
|
#diameter
|
|
vl_output[12] = 40
|
|
|
|
|
|
|
|
###############
|
|
# forward
|
|
#任务1 get
|
|
vl_output[13] = 403
|
|
#put
|
|
vl_output[14] = 406
|
|
#height
|
|
vl_output[15] = 180
|
|
#diameter
|
|
vl_output[16] = 40
|
|
|
|
#任务2 get
|
|
vl_output[17] = 406
|
|
#put
|
|
vl_output[18] = 404
|
|
#height
|
|
vl_output[19] = 180
|
|
#diameter
|
|
vl_output[20] = 40
|
|
|
|
#任务3 get
|
|
vl_output[21] = 404
|
|
#put
|
|
vl_output[22] = 102
|
|
#height
|
|
vl_output[23] = 180
|
|
#diameter
|
|
vl_output[24] = 40
|
|
|
|
return vl_output
|
|
|
|
#无水亚硫酸钠
|
|
def move6(vl_output):
|
|
#无水亚硫酸钠
|
|
|
|
vl_output[0] = 6
|
|
# # #任务1 get
|
|
vl_output[1] = 102
|
|
#put
|
|
vl_output[2] = 401
|
|
|
|
#height
|
|
vl_output[3] = 150
|
|
#diameter
|
|
vl_output[4] = 70
|
|
|
|
# # #任务2 get
|
|
vl_output[5] = 401
|
|
#put
|
|
vl_output[6] = 403
|
|
|
|
#height
|
|
vl_output[7] = 150
|
|
#diameter
|
|
vl_output[8] = 70
|
|
|
|
# # #任务3 get
|
|
vl_output[9] = 403
|
|
#put
|
|
vl_output[10] = 406
|
|
|
|
#height
|
|
vl_output[11] = 150
|
|
#diameter
|
|
vl_output[12] = 70
|
|
|
|
# # #任务4 get
|
|
vl_output[13] = 406
|
|
#put
|
|
vl_output[14] = 404
|
|
|
|
#height
|
|
vl_output[15] = 150
|
|
#diameter
|
|
vl_output[16] = 70
|
|
|
|
# # #任务5 get
|
|
vl_output[17] = 404
|
|
#put
|
|
vl_output[18] = 102
|
|
|
|
#height
|
|
vl_output[19] = 150
|
|
#diameter
|
|
vl_output[20] = 70
|
|
|
|
|
|
# # #任务6 get
|
|
vl_output[21] = 102
|
|
#put
|
|
vl_output[22] = 101
|
|
|
|
#height
|
|
vl_output[23] = 150
|
|
#diameter
|
|
vl_output[24] = 70
|
|
|
|
#混合任务
|
|
def move7(vl_output):
|
|
#混合任务
|
|
|
|
vl_output[0] = 8
|
|
# # #任务1 get
|
|
vl_output[1] = 102
|
|
#put
|
|
vl_output[2] = 401
|
|
|
|
#height
|
|
vl_output[3] = 160
|
|
#diameter
|
|
vl_output[4] = 55
|
|
|
|
# # #任务2 get
|
|
vl_output[5] = 401
|
|
#put
|
|
vl_output[6] = 403
|
|
|
|
#height
|
|
vl_output[7] = 160
|
|
#diameter
|
|
vl_output[8] = 55
|
|
|
|
# # #任务3 get
|
|
vl_output[9] = 403
|
|
#put
|
|
vl_output[10] = 406
|
|
|
|
#height
|
|
vl_output[11] = 160
|
|
#diameter
|
|
vl_output[12] = 55
|
|
|
|
# # #任务4 get
|
|
vl_output[13] = 406
|
|
#put
|
|
vl_output[14] = 404
|
|
|
|
#height
|
|
vl_output[15] = 160
|
|
#diameter
|
|
vl_output[16] = 55
|
|
|
|
# # #任务5 get
|
|
vl_output[17] = 404
|
|
#put
|
|
vl_output[18] = 102
|
|
|
|
#height
|
|
vl_output[19] = 160
|
|
#diameter
|
|
vl_output[20] = 55
|
|
|
|
|
|
# # #任务6 get
|
|
vl_output[21] = 102
|
|
#put
|
|
vl_output[22] = 101
|
|
|
|
#height
|
|
vl_output[23] = 160
|
|
#diameter
|
|
vl_output[24] = 55
|
|
|
|
|
|
#任务7 get
|
|
vl_output[25] = 206
|
|
#put
|
|
vl_output[26] = 202
|
|
#height
|
|
vl_output[27] = 117
|
|
#diameter
|
|
vl_output[28] = 50
|
|
|
|
#任务8 get
|
|
vl_output[29] = 202
|
|
#put
|
|
vl_output[30] = 205
|
|
#height
|
|
vl_output[31] = 117
|
|
#diameter
|
|
vl_output[32] = 50
|
|
|
|
|
|
|
|
# 大白瓶 单次取
|
|
def move8(vl_output):
|
|
# 大白瓶
|
|
###############
|
|
# backward
|
|
vl_output[0] = 1
|
|
#任务1 get
|
|
vl_output[1] = 402
|
|
#put
|
|
vl_output[2] = 102
|
|
#height
|
|
vl_output[3] = 180
|
|
#diameter
|
|
vl_output[4] = 40
|
|
|
|
|
|
# 大白瓶 单次放
|
|
def move9(vl_output):
|
|
# 大白瓶
|
|
###############
|
|
# backward
|
|
vl_output[0] = 1
|
|
#任务1 get
|
|
vl_output[1] = 102
|
|
#put
|
|
vl_output[2] = 402
|
|
#height
|
|
vl_output[3] = 180
|
|
#diameter
|
|
vl_output[4] = 40
|
|
|
|
@agv_router.get('/singlemove', summary="单只取放")
|
|
async def singleMove(request: Request):
|
|
global CNT
|
|
global LABELMAP
|
|
|
|
# clear all register related in PLC
|
|
clear_batch()
|
|
|
|
CNT = 0
|
|
LABELMAP = []
|
|
|
|
layerSrc = 3
|
|
end = "01-0111-03-02"
|
|
|
|
tasktype = 3
|
|
if 1 == tasktype:
|
|
# little box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapLittleBox
|
|
elif 3 == tasktype:
|
|
# big box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapBigBox
|
|
else:
|
|
print("Cannot Match")
|
|
|
|
print("LABELMAP: ", LABELMAP)
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = 5
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layerSrc
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 1
|
|
|
|
#小框
|
|
vl_output[4] = 1
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = []
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
# #混合任务
|
|
# move7(vl_output)
|
|
|
|
#大白瓶单次
|
|
move8(vl_output)
|
|
|
|
print("T2: ", vl_output)
|
|
write_task_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
|
|
if 1 == tasktype or 3 == tasktype:
|
|
returnBox(paths[-1], layerSrc)
|
|
clearCmd()
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
@agv_router.get('/simshceduler', summary="模拟调度命令")
|
|
async def simShceduler():
|
|
|
|
await clearHoles()
|
|
|
|
assignmentCmd = [
|
|
{
|
|
"01-0113-3-2": {
|
|
"cap": 40,
|
|
"name": "大白瓶",
|
|
"action": 5,
|
|
"bottle": 60,
|
|
"height": 180,
|
|
"orderid": "tr43frr",
|
|
"traynum": "1X01",
|
|
"quantity": 1,
|
|
"typetask": 3,
|
|
"coordinates": "01-0111-3-2"
|
|
}
|
|
},
|
|
{
|
|
"01-0113-3-5": {
|
|
"cap": 70,
|
|
"name": "无水亚硫酸钠",
|
|
"action": 5,
|
|
"bottle": 60,
|
|
"height": 150,
|
|
"orderid": "tr43frr",
|
|
"traynum": "1X02",
|
|
"quantity": 1,
|
|
"typetask": 3,
|
|
"coordinates": "01-0111-3-5"
|
|
}
|
|
}
|
|
]
|
|
|
|
|
|
wholeBoxLabel = None
|
|
wholeBoxTask = None
|
|
|
|
taskDic = {}
|
|
#Combine all task in same box
|
|
for task in assignmentCmd:
|
|
print("Task: ", task)
|
|
destLabel = list(task.keys())[0]
|
|
|
|
parts = task[destLabel]["coordinates"].split("-")
|
|
|
|
# change label '01-0154-1-1' -> '01-0154-1'
|
|
if len(parts) > 3:
|
|
taskLabel = "-".join(parts[0:3])
|
|
else:
|
|
wholeBoxLabel = "-".join(parts)
|
|
wholeBoxTask = task
|
|
continue
|
|
|
|
try:
|
|
taskDic[taskLabel].append(task)
|
|
except:
|
|
taskDic[taskLabel] = [task]
|
|
|
|
taskDicKeys = list(taskDic.keys())
|
|
taskDicKeys.sort()
|
|
|
|
#Whole get task will put at the end of task list
|
|
if wholeBoxLabel and wholeBoxTask:
|
|
taskDic[wholeBoxLabel] = [wholeBoxTask]
|
|
taskDicKeys.append(wholeBoxLabel)
|
|
|
|
for key in taskDicKeys:
|
|
await getAllTask(taskDic[key])
|
|
|
|
#where to put in AGV plate in getting task !!!
|
|
#where to get from AGV plate in putting task !!!
|
|
|
|
#################
|
|
### put whole task!
|
|
#################
|
|
|
|
wholeBoxLabel = None
|
|
wholeBoxTask = None
|
|
|
|
taskDic = {}
|
|
#Combine all task in same box
|
|
for task in assignmentCmd:
|
|
print("Task: ", task)
|
|
destLabel = list(task.keys())[0]
|
|
|
|
parts = destLabel.split("-")
|
|
|
|
# change label '01-0154-1-1' -> '01-0154-1'
|
|
if len(parts) > 3:
|
|
taskLabel = "-".join(parts[0:3])
|
|
else:
|
|
wholeBoxLabel = "-".join(parts)
|
|
wholeBoxTask = task
|
|
continue
|
|
|
|
try:
|
|
taskDic[taskLabel].append(task)
|
|
except:
|
|
taskDic[taskLabel] = [task]
|
|
|
|
taskDicKeys = list(taskDic.keys())
|
|
taskDicKeys.sort()
|
|
|
|
#Whole get task will put at the end of task list
|
|
if wholeBoxLabel and wholeBoxTask:
|
|
taskDic[wholeBoxLabel] = [wholeBoxTask]
|
|
taskDicKeys.insert(0, wholeBoxLabel)
|
|
|
|
for key in taskDicKeys:
|
|
await putAllTask(taskDic[key])
|
|
|
|
await clearHoles()
|
|
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
|
|
|
|
@agv_router.get('/t1', summary="接驳台整箱到货架")
|
|
async def t1():
|
|
|
|
clear_batch()
|
|
|
|
orderType = 1
|
|
layer = 2
|
|
end = "01-0067-2"
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderType
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layer
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
#######################
|
|
|
|
orderType = 2
|
|
layer = 3
|
|
end = "01-0113-3"
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderType
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layer
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T2: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
|
|
@agv_router.get('/t2', summary="货架整箱到接驳台")
|
|
async def t2():
|
|
|
|
clear_batch()
|
|
|
|
orderType = 1
|
|
layer = 3
|
|
end = "01-0113-3"
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderType
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layer
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
#######################
|
|
|
|
orderType = 2
|
|
|
|
layer = 2
|
|
end = "01-0067-2"
|
|
|
|
vl_output = []
|
|
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = orderType
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layer
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 2
|
|
|
|
#D21010-D21059
|
|
#AGV站点号
|
|
# vl_output[10] = 311
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T2: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
return respond_to(data={"status": "Hello World!"})
|
|
|
|
|
|
|
|
@agv_router.get('/t3', summary="接驳台取单只到货架")
|
|
async def t3():
|
|
clear_batch()
|
|
|
|
global CNT
|
|
global LABELMAP
|
|
|
|
# clear all register related in PLC
|
|
clear_batch()
|
|
|
|
CNT = 0
|
|
LABELMAP = []
|
|
|
|
layerSrc = 2
|
|
end = "01-0066-02-02"
|
|
|
|
tasktype = 3
|
|
if 1 == tasktype:
|
|
# little box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapLittleBox
|
|
elif 3 == tasktype:
|
|
# big box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapBigBox
|
|
else:
|
|
print("Cannot Match")
|
|
|
|
print("LABELMAP: ", LABELMAP)
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = 1
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layerSrc
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 1
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = []
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#大白瓶单次取
|
|
move8(vl_output)
|
|
|
|
print("T2: ", vl_output)
|
|
write_task_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
|
|
if 1 == tasktype or 3 == tasktype:
|
|
returnBox(paths[-1], layerSrc)
|
|
clearCmd()
|
|
|
|
#######################
|
|
|
|
|
|
CNT = 0
|
|
LABELMAP = []
|
|
|
|
layerSrc = 3
|
|
end = "01-0111-03-02"
|
|
|
|
tasktype = 3
|
|
if 1 == tasktype:
|
|
# little box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapLittleBox
|
|
elif 3 == tasktype:
|
|
# big box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapBigBox
|
|
else:
|
|
print("Cannot Match")
|
|
|
|
print("LABELMAP: ", LABELMAP)
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = 1
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layerSrc
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 1
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = []
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#大白瓶单次取
|
|
move9(vl_output)
|
|
|
|
print("T2: ", vl_output)
|
|
write_task_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
|
|
if 1 == tasktype or 3 == tasktype:
|
|
returnBox(paths[-1], layerSrc)
|
|
clearCmd()
|
|
|
|
|
|
|
|
@agv_router.get('/t4', summary="货架取单只到接驳台")
|
|
async def t4():
|
|
|
|
global CNT
|
|
global LABELMAP
|
|
|
|
clear_batch()
|
|
|
|
#######################
|
|
|
|
CNT = 0
|
|
LABELMAP = []
|
|
|
|
layerSrc = 3
|
|
end = "01-0111-03-02"
|
|
|
|
tasktype = 3
|
|
if 1 == tasktype:
|
|
# little box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapLittleBox
|
|
elif 3 == tasktype:
|
|
# big box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapBigBox
|
|
else:
|
|
print("Cannot Match")
|
|
|
|
print("LABELMAP: ", LABELMAP)
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = 1
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layerSrc
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 1
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = []
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#大白瓶单次取
|
|
move8(vl_output)
|
|
|
|
print("T2: ", vl_output)
|
|
write_task_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
|
|
if 1 == tasktype or 3 == tasktype:
|
|
returnBox(paths[-1], layerSrc)
|
|
clearCmd()
|
|
|
|
#######################
|
|
|
|
|
|
# clear all register related in PLC
|
|
clear_batch()
|
|
|
|
CNT = 0
|
|
LABELMAP = []
|
|
|
|
layerSrc = 2
|
|
end = "01-0066-02-02"
|
|
|
|
tasktype = 3
|
|
if 1 == tasktype:
|
|
# little box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapLittleBox
|
|
elif 3 == tasktype:
|
|
# big box
|
|
LABELMAP = labelMapBigDisk + labelMapLittleDisk + labelMapBigBox
|
|
else:
|
|
print("Cannot Match")
|
|
|
|
print("LABELMAP: ", LABELMAP)
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#任务类型
|
|
vl_output[0] = 1
|
|
|
|
#是否经过玻璃门
|
|
vl_output[1] = 2
|
|
|
|
#取框层数
|
|
vl_output[2] = layerSrc
|
|
|
|
#手臂是否需要动作
|
|
vl_output[3] = 1
|
|
|
|
status = readStatus()
|
|
start = status[2]
|
|
startStop = getAgvStop(start)
|
|
|
|
paths = []
|
|
paths = genPath(startStop, end)
|
|
print("Path: ", paths)
|
|
|
|
cnt = 10
|
|
for path in paths:
|
|
vl_output[cnt] = path
|
|
cnt = cnt + 1
|
|
|
|
print("T1: ", vl_output)
|
|
write_batch(vl_output)
|
|
|
|
|
|
vl_output = []
|
|
|
|
# for _ in range(100):
|
|
for _ in range(100):
|
|
vl_output.append(0)
|
|
|
|
#大白瓶单次取
|
|
move9(vl_output)
|
|
|
|
print("T2: ", vl_output)
|
|
write_task_batch(vl_output)
|
|
|
|
clearCmd()
|
|
|
|
|
|
if 1 == tasktype or 3 == tasktype:
|
|
returnBox(paths[-1], layerSrc)
|
|
clearCmd()
|
|
|
|
|
|
|
|
def getCamStatus():
|
|
global CNT
|
|
global LASTITEM
|
|
global LASTX
|
|
global LASTY
|
|
|
|
status = readCamStatus()
|
|
|
|
xStatus = list(status)
|
|
# print("getCamStatus: ", xStatus)
|
|
|
|
xValue = intList2Float(xStatus[0:2])
|
|
# print("xValue: ", xValue)
|
|
|
|
yValue = intList2Float(xStatus[2:4])
|
|
# print("yValue: ", yValue)
|
|
|
|
zValue = intList2Float(xStatus[4:6])
|
|
# print("zValue: ", zValue)
|
|
|
|
rValue = intList2Float(xStatus[6:8])
|
|
# print("rValue: ", rValue)
|
|
|
|
if 1 == xStatus[9]:
|
|
print("getCamStatus Triger {}".format(xStatus[9]))
|
|
item = getParam()
|
|
print("item: ", item)
|
|
if item:
|
|
#The oritention of camera matter the sign value
|
|
#toward AGV is 1, Backward AGV is -1
|
|
armX, armY, sign = getArmXY(xValue, yValue)
|
|
|
|
dx = item[3] * setting.DXCAMREALRATIO * sign
|
|
dy = item[4] * setting.DYCAMREALRATIO * sign
|
|
|
|
if 0 == CNT % 2:
|
|
|
|
LASTITEM = item
|
|
LASTX = xValue
|
|
LASTY = yValue
|
|
print("xValue + dx: ", xValue, dx, xValue + dx)
|
|
print("yValue + dy: ", yValue, dy, yValue + dy)
|
|
# XList = float2intList(xValue + dx)
|
|
# YList = float2intList(yValue + dy)
|
|
XList = float2intList(xValue + dy)
|
|
YList = float2intList(yValue + dx)
|
|
|
|
print("Set LASTITEM: ", LASTITEM)
|
|
else:
|
|
# armX, armY = getArmXY(xValue, yValue)
|
|
print("armX + dx: ", armX, dx, armX + dx)
|
|
print("armY + dy: ", armY, dy, armY + dy)
|
|
# XList = float2intList(armX + dx)
|
|
# YList = float2intList(armY + dy)
|
|
|
|
if LASTITEM:
|
|
distance = math.dist([item[3], item[4]], [0, 0])
|
|
distanceLast = math.dist([LASTITEM[3], LASTITEM[4]], [0, 0])
|
|
|
|
ldx = LASTITEM[3] * setting.DXCAMREALRATIO * sign
|
|
ldy = LASTITEM[4] * setting.DYCAMREALRATIO * sign
|
|
|
|
if distance < distanceLast:
|
|
|
|
XList = float2intList(armX + dy + ldy)
|
|
YList = float2intList(armY + dx + ldx)
|
|
else:
|
|
XList = float2intList(armX + ldy)
|
|
YList = float2intList(armY + ldx)
|
|
|
|
LASTITEM = None
|
|
else:
|
|
print("Get LASTITEM Error: ", LASTITEM)
|
|
# raise Exception("Miss the first item")
|
|
|
|
print("XList: {}, YList: {}".format(XList, YList))
|
|
|
|
# 拍照完成，
|
|
# 工控机返回玻璃门
|
|
res = [ 1, 2 ] + XList + YList
|
|
setCamIntList(res)
|
|
|
|
CNT = CNT + 1
|
|
|
|
time.sleep(1)
|
|
|
|
#clear cam finish when triger cam finish
|
|
clearCamFinish = [ 0 ]
|
|
setCamIntList(clearCamFinish)
|
|
|
|
else:
|
|
# XList = float2intList(LASTX)
|
|
# YList = float2intList(LASTY)
|
|
|
|
# res = [ 0, 2 ] + XList + YList
|
|
# setCamIntList(res)
|
|
|
|
print("Find No Object!")
|
|
|
|
|
|
else:
|
|
#clear cam finish when triger cam finish
|
|
clearCamFinish = [ 0 ]
|
|
setCamIntList(clearCamFinish)
|
|
|
|
|
|
def clearCmd():
|
|
#wait 1 second after send cmd
|
|
time.sleep(1)
|
|
status = readStatus()
|
|
print("getAgvStatus: ", status)
|
|
|
|
cnt = 0
|
|
while 1 != status[0]:
|
|
# print("cnt: ", cnt)
|
|
cnt = cnt + 1
|
|
time.sleep(0.1)
|
|
status = readStatus()
|
|
# print("getAgvStatus: ", status)
|
|
|
|
getCamStatus()
|
|
|
|
#PLC terminate by timeout
|
|
if cnt > setting.PLCTIMEOUT:
|
|
print("PLC Timeout, Exit!")
|
|
break
|
|
|
|
clear_batch()
|
|
print("clear_batch: ")
|
|
|
|
|
|
#get camera axis related arm axis
|
|
def getArmXY(x, y):
|
|
# labelMap = [
|
|
# #Big x, y, x1, y1, Label, num, sign
|
|
# [ 259.9213, -199.1091, 238.0209, -107.0088, "B1", 401, 1],#1
|
|
# [ 118.2232, -113.7113, 133.223, -107.9106, "B2", 404, 1],#2
|
|
# [ 298.0214, -9.1093, 242.2205, -0.7079, "B3", 402, 1],#3
|
|
# [ 187.2214, -7.0099, 138.2216, -1.9102, "B4", 405, 1],#4
|
|
# [ 298.4224, 93.9812, 246.2243, 106.5838, "B5", 403, 1],#5
|
|
# [ 195.8225, 98.182, 141.224, 108.6835, "B6", 406, 1],#6
|
|
# # #little
|
|
# # [219.2259, -134.5134, 274.2259, -139.5131, "L1", 301, -1],#1
|
|
# # [141.2238, -131.5109, 187.1251, -136.5126, "L2", 305, -1],#2
|
|
# # # [, "L3", 309, -1],#3
|
|
# # [225.2205, -44.5074, 277.2257, -51.5123, "L4", 302, -1],#4
|
|
# # [139.2199, -44.5065, 190.0238, -48.612, "L5", 306, -1], #5
|
|
# # # [, "L6", 310, -1],#6
|
|
# # [228.7183, 40.4935, 284.5179, 40.4931, "L7", 303, -1],#7
|
|
# # [142.4177, 40.4924, 198.2177, 42.5925, "L8", 307, -1],#8
|
|
# # # [, "L9", 311, -1],#9
|
|
# # [233.2122, 132.0875, 284.0166, 130.4906, "L10", 304, -1],#10
|
|
# # # [, "L11", 308, -1],#11
|
|
# # [145.0099, 132.0864, 196.1168, 132.5911, "L12", 312, -1],#12
|
|
# #big disk
|
|
# [12.2261, -215.5127, -44.7728, -205.5122, "BD1", 102, 1],#1
|
|
# [-144.6989, -209.1089, -164.6981, -204.1083, "BD2", 101, 1],#2
|
|
# #little disk
|
|
# [45.8251, 147.7807, -4.1743, 149.881, "LD1", 205, 1],#1
|
|
# [-42.3746, 145.6784, -92.3706, 154.079, "LD2", 203, 1],#2
|
|
# [-130.5713, 151.79761, -180.57, 154.0777, "LD3", 201, 1],#3
|
|
# [48.6326, 246.1763, -2.0666, 248.9752, "LD4", 206, 1],#4
|
|
# [-39.5668, 248.2769, -89.1668, 253.9755, "LD5", 204, 1],#5
|
|
# [-125.6693, 248.2777, -174.2673, 254.8755, "LD6", 202, 1],#6
|
|
# ]
|
|
|
|
global LABELMAP
|
|
|
|
#find nearest
|
|
LABELMAP.sort(key = lambda item: math.dist([x, y], [ item[0], item[1] ] ))
|
|
# print("labelMap: ", labelMap)
|
|
nearestItem = LABELMAP[0]
|
|
#return corresponding arm x and y value
|
|
return nearestItem[2], nearestItem[3], nearestItem[6]
|