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]