四向穿梭车逻辑优化

Junjie

2023-04-08 0b83448133a1af459273318bc5b0c2a4bc6e0448

 src/main/java/com/zy/core/thread/ShuttleThread.java

@@ -10,8 +10,10 @@
import com.core.exception.CoolException;
import com.zy.asrs.entity.BasShuttle;
import com.zy.asrs.entity.BasShuttleOpt;
import com.zy.asrs.entity.LocMast;
import com.zy.asrs.service.BasShuttleOptService;
import com.zy.asrs.service.BasShuttleService;
import com.zy.asrs.service.LocMastService;
import com.zy.common.model.NavigateNode;
import com.zy.common.utils.CommonUtils;
import com.zy.common.utils.NavigatePositionConvert;
@@ -21,10 +23,10 @@
import com.zy.core.ThreadHandler;
import com.zy.core.cache.MessageQueue;
import com.zy.core.cache.OutputQueue;
import com.zy.core.cache.SlaveConnection;
import com.zy.core.enums.*;
import com.zy.core.model.ShuttleSlave;
import com.zy.core.model.Task;
import com.zy.core.model.command.LiftCommand;
import com.zy.core.model.command.ShuttleAssignCommand;
import com.zy.core.model.command.ShuttleCommand;
import com.zy.core.model.protocol.ShuttleProtocol;
@@ -137,7 +139,12 @@
                //错误编号
                shuttleProtocol.setErrorCode(modbusTcpNet.getByteTransform().TransInt16(content,8));
                //Plc输出状态IO
                shuttleProtocol.setPlcOutputStatusIO(modbusTcpNet.getByteTransform().TransInt16(content,10));
                boolean[] booleans = modbusTcpNet.getByteTransform().TransBool(content, 10, 2);
                shuttleProtocol.setPlcOutputLift(booleans[1]);
                shuttleProtocol.setPlcOutputTransfer(booleans[2]);
                shuttleProtocol.setPlcOutputBrake(booleans[3]);
                shuttleProtocol.setPlcOutputCharge(booleans[4]);
                shuttleProtocol.setPlcOutputStatusIO(modbusTcpNet.getByteTransform().TransInt16(content, 10));
                //错误信息码
                shuttleProtocol.setStatusErrorCode(modbusTcpNet.getByteTransform().TransInt16(content,12));
                //PLC输入状态
@@ -166,6 +173,20 @@
                //小车处于忙碌状态，将标记置为true
                if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.BUSY) {
                    shuttleProtocol.setPakMk(true);
                }

                if (shuttleProtocol.getErrorCode() != 0 && shuttleProtocol.getProtocolStatusType() == ShuttleProtocolStatusType.WORKING) {
                    //出现错误
                    reset(shuttleProtocol.getAssignCommand());
                }

                if(shuttleProtocol.getProtocolStatusType() == ShuttleProtocolStatusType.FIXING
                        && shuttleProtocol.getTaskNo() != 0
                        && shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE
                        && shuttleProtocol.getAssignCommand() != null){
                    //处于故障修复状态
                    //执行下一步指令
                    executeWork(shuttleProtocol.getAssignCommand());
                }

                //四向穿梭车空闲、有任务、标记为true、存在任务指令，需要执行任务的下一条指令
@@ -349,39 +370,20 @@
    private void assignWork(ShuttleAssignCommand assignCommand) {
        //将此map存入redis中
        HashMap<String, Object> map = new HashMap<>();

//        if (assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_IN.id.shortValue()
//                || assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_OUT.id.shortValue()) {
//            //入库或出库模式，计算路径
//            //计算路径
//            List<NavigateNode> calc = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), assignCommand.getTaskMode().intValue());
//            if (calc != null) {
//                //获取分段路径
//                ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
//                //路径数据
//                map.put("path", data);
//                //路径次数
//                map.put("pathSize", data.size());
//            }else {
//                if (assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_IN.id.shortValue() || assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_OUT.id.shortValue()) {
//                    //属于入库出库，必须要计算路径，则抛出异常
//                    throw new Exception("未能找到运行路径");
//                }
//            }
//        }

        if (!assignCommand.getAuto()) {
            List<ShuttleCommand> commands = new ArrayList<>();
            ShuttleCommand command = new ShuttleCommand();
            LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
            switch (assignCommand.getTaskMode()) {
                case 1://入库
                case 2://出库
                    //小车移动到提升机口，计算路径
                    //计算小车起点到中点所需命令
                    List<NavigateNode> calc = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    if (calc != null) {
                    LocMast currentLocMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
                    List<NavigateNode> firstMastResult = NavigateUtils.calc(currentLocMast.getLocNo(), assignCommand.getSourceLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    if (firstMastResult != null) {
                        //获取分段路径
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(firstMastResult);
                        //将每一段路径分成command指令
                        for (ArrayList<NavigateNode> nodes : data) {
                            //开始路径
@@ -392,27 +394,108 @@

                            ShuttleCommand command1 = new ShuttleCommand();
                            command1.setCommandWord((short) 1);
                            command1.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY()));
                            command1.setMiddleCodeNum((short) 0);
                            command1.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY()));
                            command1.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), 1));
                            command1.setMiddleCodeNum((short) 1);
                            command1.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), 1));
                            command1.setStartToDistDistance(allDistance);
                            command1.setMiddleToDistDistance(1000);
                            command1.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
                            command1.setPalletLift((short) 1);
                            command1.setForceMoveDistance(1000);
                            command1.setChargeSwitch((short) 2);
                            command1.setForceMoveDistance(0);
                            command1.setIOControl((short) 0);
                            command1.setRunSpeed((short) 1000);
                            command1.setRadarTmp((short) 0);
                            command1.setCommandEnd((short) 1);
                            commands.add(command1);
                        }

                        //托盘顶升
                        ShuttleCommand command2 = new ShuttleCommand();
                        command2.setCommandWord((short) 2);
                        command2.setStartCodeNum((short) 0);
                        command2.setMiddleCodeNum((short) 0);
                        command2.setDistCodeNum((short) 0);
                        command2.setStartToDistDistance(0);
                        command2.setMiddleToDistDistance(0);
                        command2.setRunDirection((short) 0);
                        command2.setForceMoveDistance(0);
                        command2.setPalletLift((short) 1);
                        command2.setRunSpeed((short) 0);
                        command2.setCommandEnd((short) 1);
                        commands.add(command2);
                    }else {
                        //没有计算到路径，可能存在小车位置就是起点位置
                        if (currentLocMast.getLocNo().equals(assignCommand.getSourceLocNo())) {
                            //小车位置就是起点位置，无需移动，直接顶升
                            //托盘顶升
                            ShuttleCommand command2 = new ShuttleCommand();
                            command2.setCommandWord((short) 2);
                            command2.setStartCodeNum((short) 0);
                            command2.setMiddleCodeNum((short) 0);
                            command2.setDistCodeNum((short) 0);
                            command2.setStartToDistDistance(0);
                            command2.setMiddleToDistDistance(0);
                            command2.setRunDirection((short) 0);
                            command2.setForceMoveDistance(0);
                            command2.setPalletLift((short) 1);
                            command2.setRunSpeed((short) 0);
                            command2.setCommandEnd((short) 1);
                            commands.add(command2);
                        }
                    }

                    //计算中点到终点路径
                    List<NavigateNode> secMastResult = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    if (secMastResult != null) {
                        //获取分段路径
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(secMastResult);
                        //将每一段路径分成command指令
                        for (ArrayList<NavigateNode> nodes : data) {
                            //开始路径
                            NavigateNode startPath = nodes.get(0);
                            //目标路径
                            NavigateNode endPath = nodes.get(nodes.size() - 1);
                            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离

                            ShuttleCommand command1 = new ShuttleCommand();
                            command1.setCommandWord((short) 1);
                            command1.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), 1));
                            command1.setMiddleCodeNum((short) 1);
                            command1.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), 1));
                            command1.setStartToDistDistance(allDistance);
                            command1.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
                            command1.setForceMoveDistance(0);
                            command1.setIOControl((short) 0);
                            command1.setRunSpeed((short) 1000);
                            command1.setCommandEnd((short) 1);
                            commands.add(command1);
                        }

                        //托盘下降
                        ShuttleCommand command2 = new ShuttleCommand();
                        command2.setCommandWord((short) 2);
                        command2.setStartCodeNum((short) 0);
                        command2.setMiddleCodeNum((short) 0);
                        command2.setDistCodeNum((short) 0);
                        command2.setStartToDistDistance(0);
                        command2.setMiddleToDistDistance(0);
                        command2.setRunDirection((short) 0);
                        command2.setForceMoveDistance(0);
                        command2.setPalletLift((short) 2);
                        command2.setRunSpeed((short) 0);
                        command2.setCommandEnd((short) 1);
                        commands.add(command2);
                    }
                    break;
                case 3://托盘顶升
                case 4://托盘下降
                    command.setCommandWord((short) 2);
                    command.setStartCodeNum((short) 0);
                    command.setMiddleCodeNum((short) 0);
                    command.setDistCodeNum((short) 0);
                    command.setStartToDistDistance(0);
                    command.setMiddleToDistDistance(0);
                    command.setRunDirection((short) 0);
                    command.setForceMoveDistance(0);
                    command.setPalletLift(assignCommand.getTaskMode() == 3 ? (short)1 : (short)2);
                    command.setRunSpeed((short) 0);
                    command.setCommandEnd((short) 1);
                    commands.add(command);
                    break;
@@ -421,13 +504,99 @@
                case 7://强制上移
                case 8://强制下移
                    command.setCommandWord((short) 3);
                    command.setForceMoveDistance(1000);
                    command.setStartCodeNum((short) 0);
                    command.setMiddleCodeNum((short) 0);
                    command.setDistCodeNum((short) 0);
                    command.setStartToDistDistance(600);
                    command.setRunDirection((short) (assignCommand.getTaskMode() - 4));
                    command.setForceMoveDistance(600);
                    command.setIOControl((short) 0);
                    command.setCommandEnd((short) 1);
                    command.setRunSpeed((short) 1000);
                    commands.add(command);
                    break;
                case 9://状态复位
                    command.setCommandWord((short) 0);
                    command.setCommandWord((short) 6);
                    command.setStartCodeNum((short) 0);
                    command.setMiddleCodeNum((short) 0);
                    command.setDistCodeNum((short) 0);
                    command.setStartToDistDistance(0);
                    command.setMiddleToDistDistance(0);
                    command.setRunDirection((short) 0);
                    command.setPalletLift((short) 0);
                    command.setPalletLift((short) 0);
                    command.setForceMoveDistance(0);
                    command.setChargeSwitch((short) 0);
                    command.setIOControl((short) 0);
                    command.setRunSpeed((short) 0);
                    command.setCommandEnd((short) 1);
                    commands.add(command);
                    break;
                case 10://向正方向(左)寻库位
                    command.setCommandWord((short) 4);
                    command.setRunDirection((short) 1);
                    command.setStartToDistDistance(3000);
                    command.setRunSpeed((short) 1000);
                    command.setCommandEnd((short) 1);
                    commands.add(command);
                    break;
                case 11://向负方向(右)寻库位
                    command.setCommandWord((short) 4);
                    command.setRunDirection((short) 2);
                    command.setStartToDistDistance(3000);
                    command.setRunSpeed((short) 1000);
                    command.setCommandEnd((short) 1);
                    commands.add(command);
                    break;
                case 12://向正方向(前)寻库位
                    command.setCommandWord((short) 4);
                    command.setRunDirection((short) 4);
                    command.setStartToDistDistance(3000);
                    command.setRunSpeed((short) 1000);
                    command.setCommandEnd((short) 1);
                    commands.add(command);
                    break;
                case 13://向负方向(后)寻库位
                    command.setCommandWord((short) 4);
                    command.setRunDirection((short) 3);
                    command.setStartToDistDistance(3000);
                    command.setRunSpeed((short) 1000);
                    command.setCommandEnd((short) 1);
                    commands.add(command);
                    break;
                case 14://移动到目标库位
                    LocMast locMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    if (result != null) {
                        //获取分段路径
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(result);
                        //将每一段路径分成command指令
                        for (ArrayList<NavigateNode> nodes : data) {
                            //开始路径
                            NavigateNode startPath = nodes.get(0);
                            //目标路径
                            NavigateNode endPath = nodes.get(nodes.size() - 1);
                            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离

                            LocMast distLocMast = locMastService.queryByLoc(assignCommand.getLocNo());
                            String qrCodeValue = distLocMast.getQrCodeValue();
                            command.setCommandWord((short) 1);
                            command.setStartCodeNum(shuttleProtocol.getCurrentCode());
                            command.setMiddleCodeNum((short) 1);
                            command.setDistCodeNum((short) Integer.parseInt(qrCodeValue));
                            command.setStartToDistDistance(allDistance);
                            command.setRunSpeed((short) 1000);
                            command.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
                            command.setForceMoveDistance(0);
                            command.setIOControl((short) 0);
                            command.setCommandEnd((short) 1);
                            commands.add(command);
                        }
                    }
                    break;
                case 15://充电开关
                    command.setCommandWord((short) 8);
                    command.setIOControl((short) 4);
                    command.setCommandEnd((short) 1);
                    commands.add(command);
                    break;
@@ -444,6 +613,8 @@
        map.put("commandStep", 0);
        //命令
        map.put("assignCommand", assignCommand);
        //发生错误时尝试执行的指令，优先级最高
        map.put("errorCommands", new ArrayList<ShuttleCommand>());
        shuttleProtocol.setTaskNo(assignCommand.getTaskNo());
        shuttleProtocol.setAssignCommand(assignCommand);
        shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
@@ -468,6 +639,80 @@
            return false;
        }
        HashMap map = JSON.parseObject(o.toString(), HashMap.class);

        List<ShuttleCommand> errorCommands =  JSON.parseArray(map.get("errorCommands").toString(),ShuttleCommand.class);
        if (errorCommands.size() > 0) {
            //优先执行该指令
            ShuttleCommand errorCommand = errorCommands.get(0);//取出指令

            if(errorCommand.getCommandWord() == 1){//正常行走命令，需要先执行完找库位命令后，再执行
                LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
                LocMast locMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
                LocMast distLocMast = locMastService.queryByQrCode(errorCommand.getStartCodeNum().toString());
                if (shuttleProtocol.getCurrentCode().equals(errorCommand.getStartCodeNum())) {
                    //起点和终点属于同一库位，无需再执行移动操作
                    errorCommands.remove(0);//移除该命令
                    map.put("errorCommands", new ArrayList<ShuttleCommand>());
                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
                    //当前步序
                    int commandStep = Integer.parseInt(map.get("commandStep").toString());
                    //步序回退
                    commandStep--;
                    map.put("commandStep", commandStep);
                    //任务数据保存到redis
                    redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map));
                    shuttleProtocol.setPakMk(true);
                    return true;
                }else {
                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), distLocMast.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    if (result != null) {
                        //获取分段路径
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(result);
                        //将每一段路径分成command指令
                        for (ArrayList<NavigateNode> nodes : data) {
                            //开始路径
                            NavigateNode startPath = nodes.get(0);
                            //目标路径
                            NavigateNode endPath = nodes.get(nodes.size() - 1);
                            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离

                            String qrCodeValue = distLocMast.getQrCodeValue();
                            errorCommand.setCommandWord((short) 1);
                            errorCommand.setStartCodeNum(shuttleProtocol.getCurrentCode());
                            errorCommand.setMiddleCodeNum((short) 1);
                            errorCommand.setDistCodeNum((short) Integer.parseInt(qrCodeValue));
                            errorCommand.setStartToDistDistance(allDistance);
                            errorCommand.setRunSpeed((short) 1000);
                            errorCommand.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
                            errorCommand.setForceMoveDistance(0);
                            errorCommand.setIOControl((short) 0);
                            errorCommand.setCommandEnd((short) 1);
                            break;
                        }
                    }
                }

                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
                //当前步序
                int commandStep = Integer.parseInt(map.get("commandStep").toString());
                //步序回退
                commandStep--;
                map.put("commandStep", commandStep);
            }

            if (!write(errorCommand)) {
                News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand));
                return false;
            } else {
                News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand));
                errorCommands.remove(0);
                map.put("errorCommands", errorCommands);
                //任务数据保存到redis
                redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map));
                return true;
            }
        }

        List<ShuttleCommand> commands = assignCommand.getCommands();
        //当前步序
        int commandStep = Integer.parseInt(map.get("commandStep").toString());
@@ -476,6 +721,15 @@

        //取出命令
        ShuttleCommand command = commands.get(commandStep);

        if (assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_IN.id.shortValue()
            || assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_OUT.id.shortValue()
        ) {
            //小车失去坐标，禁止下发命令
            if (shuttleProtocol.getCurrentCode() == 0) {
                return false;
            }
        }

        //下发命令
        if (!write(command)) {
@@ -543,6 +797,91 @@
        return true;
    }

    /**
     * 复位并尝试修复错误
     */
    private boolean reset(ShuttleAssignCommand assignCommand) {
        //读取redis数据
        if (assignCommand == null) {
            return false;
        }

        Object o = redisUtil.get("wrk_no_" + assignCommand.getTaskNo());
        if (o == null) {
            return false;
        }

        HashMap map = JSON.parseObject(o.toString(), HashMap.class);
        List<ShuttleCommand> commands = assignCommand.getCommands();
        //当前步序
        int commandStep = Integer.parseInt(map.get("commandStep").toString());
        //path路径数目
        int size = commands.size();

        ArrayList<ShuttleCommand> list = new ArrayList<>();

        //取出命令
        ShuttleCommand command = commands.get(commandStep - 1);

        //复位命令
        ShuttleCommand resetCommand = new ShuttleCommand();
        resetCommand.setCommandWord((short) 6);
        resetCommand.setStartCodeNum((short) 0);
        resetCommand.setMiddleCodeNum((short) 0);
        resetCommand.setDistCodeNum((short) 0);
        resetCommand.setStartToDistDistance(0);
        resetCommand.setMiddleToDistDistance(0);
        resetCommand.setRunDirection((short) 0);
        resetCommand.setPalletLift((short) 0);
        resetCommand.setPalletLift((short) 0);
        resetCommand.setForceMoveDistance(0);
        resetCommand.setChargeSwitch((short) 0);
        resetCommand.setIOControl((short) 0);
        resetCommand.setRunSpeed((short) 0);
        resetCommand.setCommandEnd((short) 1);
        list.add(resetCommand);

        //车辆空闲，等待写入找库位命令
        //找库位命令
        ShuttleCommand searchCommand = new ShuttleCommand();
        searchCommand.setCommandWord((short) 4);
        short direction = 1;
        switch (command.getRunDirection()) {
            case 1:
                direction = 2;
                break;
            case 2:
                direction = 1;
                break;
            case 3:
                direction = 4;
                break;
            case 4:
                direction = 3;
                break;
            default:
                direction = 1;
        }

        searchCommand.setRunDirection(direction);//运行方向
        searchCommand.setStartToDistDistance(1200);
        searchCommand.setRunSpeed((short) 1000);
        searchCommand.setCommandEnd((short) 1);
        list.add(searchCommand);

        //移动车辆，需要在执行完寻找定位点后再执行
        ShuttleCommand moveCommand = new ShuttleCommand();
        moveCommand.setCommandWord((short) 1);
        moveCommand.setStartCodeNum(command.getStartCodeNum());//存入目标库位号
        list.add(moveCommand);

        map.put("errorCommands", list);
        //任务数据保存到redis
        redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map));
        shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.FIXING);
        return true;
    }

    /******************************************************************************************/
    /**************************************** 测试专用 *****************************************/
    /*****************************************************************************************/