#

Junjie

2024-04-17 c036b0582e39b53516d88f26df587b5a61c4b2f2

 zy-asrs-wcs/src/main/java/com/zy/asrs/wcs/core/kernel/command/ShuttleCommandService.java

@@ -1,10 +1,12 @@
package com.zy.asrs.wcs.core.kernel.command;

import com.alibaba.fastjson.JSON;
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import com.zy.asrs.framework.common.Cools;
import com.zy.asrs.framework.exception.CoolException;
import com.zy.asrs.wcs.core.action.LiftAction;
import com.zy.asrs.wcs.core.action.ShuttleAction;
import com.zy.asrs.wcs.core.entity.BasShuttle;
import com.zy.asrs.wcs.core.entity.Loc;
import com.zy.asrs.wcs.core.model.NavigateNode;
import com.zy.asrs.wcs.core.model.command.ShuttleAssignCommand;
@@ -28,10 +30,7 @@
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.Optional;
import java.util.*;

/**
 * Created by vincent on 2023/10/23
@@ -82,9 +81,11 @@

        ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
        assignCommand.setShuttleNo(deviceNo);
        assignCommand.setTaskNo(motion.getWrkNo());
        assignCommand.setTaskNo(motion.getTaskNo());
        assignCommand.setDeviceTaskNo(shuttleThread.generateDeviceTaskNo(motion.getTaskNo(), MotionCtgType.get(motion.getMotionCtgEl())));
        assignCommand.setSourceLocNo(motion.getOrigin());
        assignCommand.setLocNo(motion.getTarget());
        assignCommand.setDeviceId(Integer.parseInt(motion.getDevice()));

        List<ShuttleCommand> shuttleCommands = new ArrayList<>();
        ShuttleTaskModeType shuttleTaskModeType = null;
@@ -97,7 +98,7 @@
                // 如果已经在当前条码则过滤
                if (String.valueOf(shuttleProtocol.getCurrentCode()).equals(locService.getOne(new LambdaQueryWrapper<Loc>()
                        .eq(Loc::getLocNo, motion.getTarget())
                        .eq(Loc::getHostId, motion.getHostId())).getCode())) {
                            .eq(Loc::getHostId, motion.getHostId())).getCode())) {
                    return true;
                }
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
@@ -106,12 +107,12 @@
            case SHUTTLE_MOVE_LIFT_PALLET://穿梭车顶升并移动
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.DFX.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.PAK_IN;
                shuttleCommands.add(0, shuttleThread.getLiftCommand(motion.getWrkNo(), true));
                shuttleCommands.add(0, shuttleThread.getLiftCommand(motion.getTaskNo(), true));
                break;
            case SHUTTLE_MOVE_DOWN_PALLET://穿梭车移动并托盘下降
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.DFX.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.PAK_IN;
                shuttleCommands.add(shuttleCommands.size(), shuttleThread.getLiftCommand(motion.getWrkNo(), false));
                shuttleCommands.add(shuttleCommands.size(), shuttleThread.getLiftCommand(motion.getTaskNo(), false));
                break;
            case SHUTTLE_MOVE_FROM_LIFT://出提升机
                // 判断提升机状态
@@ -151,7 +152,7 @@
//                }

                //判断提升机工作号是否和当前任务相同
                if (liftProtocol.getTaskNo().intValue() != motion.getWrkNo()) {
                if (liftProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
                    return false;
                }

@@ -188,7 +189,7 @@
//                }

                //判断提升机工作号是否和当前任务相同
                if (liftProtocol.getTaskNo().intValue() != motion.getWrkNo()) {
                if (liftProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
                    return false;
                }

@@ -197,21 +198,21 @@
                break;
            case SHUTTLE_CHARGE_ON://充电开
                shuttleTaskModeType = ShuttleTaskModeType.CHARGE;
                shuttleCommands.add(shuttleThread.getChargeCommand(motion.getWrkNo(), true));
                shuttleCommands.add(shuttleThread.getChargeCommand(motion.getTaskNo(), true));
                assignCommand.setCharge(Boolean.TRUE);
                break;
            case SHUTTLE_CHARGE_OFF://充电关
                shuttleTaskModeType = ShuttleTaskModeType.CHARGE;
                shuttleCommands.add(shuttleThread.getChargeCommand(motion.getWrkNo(), false));
                shuttleCommands.add(shuttleThread.getChargeCommand(motion.getTaskNo(), false));
                assignCommand.setCharge(Boolean.TRUE);
                break;
            case SHUTTLE_PALLET_LIFT://托盘顶升
                shuttleTaskModeType = ShuttleTaskModeType.PALLET_LIFT;
                shuttleCommands.add(shuttleThread.getLiftCommand(motion.getWrkNo(), true));
                shuttleCommands.add(shuttleThread.getLiftCommand(motion.getTaskNo(), true));
                break;
            case SHUTTLE_PALLET_DOWN://托盘下降
                shuttleTaskModeType = ShuttleTaskModeType.PALLET_DOWN;
                shuttleCommands.add(shuttleThread.getLiftCommand(motion.getWrkNo(), false));
                shuttleCommands.add(shuttleThread.getLiftCommand(motion.getTaskNo(), false));
                break;
            default:
                throw new CoolException(motion.getMotionCtgEl() + "没有指定任务作业流程！！！");
@@ -245,16 +246,16 @@
            return false;
        }

        if (shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getTaskNo().intValue() != motion.getWrkNo()) {
        if (shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
            return false;
        }

        //充电任务
        if (Objects.requireNonNull(MotionCtgType.get(motion.getMotionCtgEl())).equals(MotionCtgType.SHUTTLE_CHARGE_ON)) {
            // 复位穿梭车
            shuttleProtocol.setTaskNo(0);
            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
            shuttleProtocol.setPakMk(true);
            shuttleThread.setSyncTaskNo(0);
            shuttleThread.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
            shuttleThread.setPakMk(true);
            return true;
        }

@@ -308,7 +309,7 @@
//                }

                //判断提升机工作号是否和当前任务相同
                if (liftProtocol.getTaskNo().intValue() != motion.getWrkNo()) {
                if (liftProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
                    return false;
                }

@@ -323,23 +324,27 @@
        }

        // 复位穿梭车
        shuttleProtocol.setTaskNo(0);
        shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
        shuttleProtocol.setPakMk(true);
        shuttleThread.setSyncTaskNo(0);
        shuttleThread.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
        shuttleThread.setPakMk(true);

        return true;
    }

    public synchronized List<ShuttleCommand> shuttleAssignCommand(String startLocNo, String endLocNo, Integer mapType, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
        //获取小车移动速度
        Integer runSpeed = Optional.ofNullable(basShuttleService.getById(assignCommand.getShuttleNo()).getRunSpeed()).orElse(1000);
        Integer runSpeed = Optional.ofNullable(basShuttleService.getOne(new LambdaQueryWrapper<BasShuttle>().eq(BasShuttle::getDeviceId, assignCommand.getDeviceId())).getRunSpeed()).orElse(1000);
        Long hostId = shuttleThread.getDevice().getHostId();
        List<NavigateNode> nodeList = NavigateUtils.calc(startLocNo, endLocNo, mapType, Utils.getShuttlePoints(Integer.parseInt(shuttleThread.getDevice().getDeviceNo()), Utils.getLev(startLocNo)));
        if (nodeList == null) {
            News.error("{} dash {} can't find navigate path!", startLocNo, endLocNo);
            return null;
        }
        List<NavigateNode> allNode = new ArrayList<>(nodeList);

        List<NavigateNode> allNode = new ArrayList<>();
        for (NavigateNode node : nodeList) {
            allNode.add(node.clone());
        }

        List<ShuttleCommand> commands = new ArrayList<>();
        //获取分段路径
@@ -349,25 +354,6 @@
            //开始路径
            NavigateNode startPath = nodes.get(0);

            //中间路径
            NavigateNode middlePath = null;
            //通过xy坐标小车二维码
            String middleCodeNum = null;
            Integer middleToDistDistance = null;//计算中间点到目标点行走距离
            if (nodes.size() > 10) {//中段码传倒数第三个
                //中间路径
                middlePath = nodes.get(nodes.size() - 3);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ(), hostId);
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            } else if (nodes.size() > 5) {//中段码传倒数第二个
                //中间路径
                middlePath = nodes.get(nodes.size() - 2);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ(), hostId);
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            }

            //目标路径
            NavigateNode endPath = nodes.get(nodes.size() - 1);
            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
@@ -376,7 +362,7 @@
            //通过xy坐标小车二维码
            String distCodeNum = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ(), hostId);
            //获取移动命令
            ShuttleCommand command = shuttleThread.getMoveCommand(assignCommand.getTaskNo().intValue(), startCodeNum, distCodeNum, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id.intValue(), runSpeed);
            ShuttleCommand command = shuttleThread.getMoveCommand(assignCommand.getDeviceTaskNo(), startCodeNum, distCodeNum, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id.intValue(), runSpeed);
            command.setNodes(nodes);//将行走节点添加到每一步命令中
            commands.add(command);
        }