穿梭车到达提升机口前，优先调度提升机优化

Junjie

2023-04-14 f57741a11f0cce1f7ac0f2c8ab0627c046cb855b

 src/main/java/com/zy/asrs/service/impl/MainServiceImpl.java

@@ -23,10 +23,7 @@
import com.zy.core.enums.*;
import com.zy.core.model.*;
import com.zy.core.model.command.*;
import com.zy.core.model.protocol.CrnProtocol;
import com.zy.core.model.protocol.ShuttleProtocol;
import com.zy.core.model.protocol.StaProtocol;
import com.zy.core.model.protocol.SteProtocol;
import com.zy.core.model.protocol.*;
import com.zy.core.properties.SlaveProperties;
import com.zy.core.thread.*;
import lombok.extern.slf4j.Slf4j;
@@ -70,6 +67,10 @@
    private BasErrLogService basErrLogService;
    @Autowired
    private BasSteErrLogService basSteErrLogService;
    @Autowired
    private BasShuttleErrLogService basShuttleErrLogService;
    @Autowired
    private BasShuttleErrService basShuttleErrService;
    @Autowired
    private BasCrnErrorMapper basCrnErrorMapper;
    @Autowired
@@ -605,10 +606,609 @@
        }
    }

//    /**
//     * 入库  ===>>  四向穿梭车入库作业下发
//     */
//    public synchronized void shuttleIoInExecute() {
//        // 根据输送线plc遍历
//        for (DevpSlave devp : slaveProperties.getDevp()) {
//            // 遍历入库站
//            for (DevpSlave.StaRack rackInStn : devp.getRackInStn()) {
//                // 获取入库站信息
//                DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, devp.getId());
//
//                StaProtocol staProtocol = devpThread.getStation().get(rackInStn.getStaNo());
//                StaProtocol staProtocol105 = devpThread.getStation().get(105);
//                staProtocol105.setWorkNo((short) 752);
//                staProtocol105.setStaNo((short) 100);
//
//                StaProtocol staProtocol106 = devpThread.getStation().get(106);
//                staProtocol106.setWorkNo((short) 753);
//                staProtocol106.setStaNo((short) 100);
//
//                if (staProtocol == null) {
//                    continue;
//                } else {
//                    staProtocol = staProtocol.clone();
//                }
//                Short workNo = staProtocol.getWorkNo();
//
//                // 判断是否满足入库条件
//                if (true || staProtocol.isAutoing() && staProtocol.isLoading() && staProtocol.isInEnable()) {
//                    WrkMast wrkMast = wrkMastMapper.selectRackInStep48(workNo, staProtocol.getSiteId());
//                    if (wrkMast != null) {
//                        if (wrkMast.getWrkSts() == 4 || wrkMast.getWrkSts() == 8) {
//                            ShuttleThread shuttleThread = null;
//                            HashMap<String, Object> searchIdleShuttle = null;
//                            if (wrkMast.getWrkSts() == 4) {
//                                //寻找最近且空闲的四向穿梭车
//                                searchIdleShuttle = this.searchIdleShuttle(wrkMast);
//                                shuttleThread = (ShuttleThread) searchIdleShuttle.get("result");
//                            }else {
//                                //状态8，四向穿梭车已在提升机口，等待命令进行入库搬运动作
//                                Integer shuttleNo = wrkMast.getShuttleNo();//四向穿梭车号
//                                shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttleNo);
//                            }
//
//                            if (shuttleThread == null) {
//                                continue;
//                            }
//
//                            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
//                            if (!shuttleProtocol.isIdle()) {
//                                continue;
//                            }
//
//                            wrkMast.setShuttleNo(shuttleProtocol.getShuttleNo().intValue());//给工作档分配四向穿梭车号
//
//                            //分配任务号
//                            shuttleProtocol.setTaskNo(wrkMast.getWrkNo().shortValue());
//                            //分配源库位
//                            shuttleProtocol.setSourceLocNo(wrkMast.getSourceLocNo());
//
//                            ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
//                            //四向穿梭车号
//                            assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
//                            //任务号
//                            assignCommand.setTaskNo(wrkMast.getWrkNo().shortValue());
//                            //入出库模式
//                            assignCommand.setTaskMode(ShuttleTaskModeType.PAK_IN.id.shortValue());
//                            //源库位(小车当前位置)
//                            String currentLocNo = shuttleProtocol.getCurrentLocNo();
//                            assignCommand.setSourceLocNo(currentLocNo);
//
//                            if (wrkMast.getWrkSts() == 8 || Boolean.parseBoolean(searchIdleShuttle.get("sameLay").toString())) {
//                                //同一层
//                                //分配目标库位
//                                shuttleProtocol.setLocNo(wrkMast.getLocNo());
//                                //目标库位
//                                assignCommand.setLocNo(wrkMast.getLocNo());
//                                wrkMast.setWrkSts(9L);//小车入库中
//
//                                //获取从小车
//                            }else {
//                                //不同层，将目标库位分配成提升机库位号
//
//                                //小车当前层高
//                                Integer currentLev = Integer.parseInt(currentLocNo.substring(currentLocNo.length() - 2, currentLocNo.length()));
//
//                                //获取提升机
//                                LiftSlave liftSlave = slaveProperties.getLift().get(0);
//                                //提升机库位号
//                                String liftLocNo = liftSlave.getLiftLocNo(currentLev);
//                                shuttleProtocol.setLocNo(liftLocNo);
//                                //目标库位
//                                assignCommand.setLocNo(liftLocNo);
//                                wrkMast.setWrkSts(5L);//小车迁移状态
//                            }
//
//                            if (wrkMastMapper.updateById(wrkMast) > 0) {
//                                //下发任务
//                                MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
//                            }
//                        }
//
//                    }
//                }
//
//            }
//        }
//
//    }

    /**
     * 入出库  ===>>  四向穿梭车入出库作业下发
     * 入库  ===>>  四向穿梭车入库作业下发
     */
    public synchronized void shuttleIoExecute() {
    public synchronized void shuttleIoInExecute() {
        // 根据输送线plc遍历
        for (DevpSlave devp : slaveProperties.getDevp()) {
            // 遍历入库站
            for (DevpSlave.StaRack rackInStn : devp.getRackInStn()) {
                // 获取入库站信息
                DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, devp.getId());

                StaProtocol staProtocol = devpThread.getStation().get(rackInStn.getStaNo());
                StaProtocol staProtocol105 = devpThread.getStation().get(105);
                staProtocol105.setWorkNo((short) 752);
                staProtocol105.setStaNo((short) 100);

                StaProtocol staProtocol106 = devpThread.getStation().get(106);
                staProtocol106.setWorkNo((short) 753);
                staProtocol106.setStaNo((short) 100);

                if (staProtocol == null) {
                    continue;
                } else {
                    staProtocol = staProtocol.clone();
                }
                Short workNo = staProtocol.getWorkNo();

                // 判断是否满足入库条件
                if (true || staProtocol.isAutoing() && staProtocol.isLoading() && staProtocol.isInEnable()) {
                    WrkMast wrkMast = wrkMastMapper.selectRackInStep48(workNo, staProtocol.getSiteId());
                    if (wrkMast != null) {
                        if (wrkMast.getWrkSts() == 4 || wrkMast.getWrkSts() == 8) {
                            ShuttleThread shuttleThread = null;
                            HashMap<String, Object> searchIdleShuttle = null;
                            if (wrkMast.getWrkSts() == 4) {
                                //寻找最近且空闲的四向穿梭车
                                searchIdleShuttle = this.searchIdleShuttle(wrkMast);
                                shuttleThread = (ShuttleThread) searchIdleShuttle.get("result");
                            }else {
                                //状态8，四向穿梭车已在提升机口，等待命令进行入库搬运动作
                                Integer shuttleNo = wrkMast.getShuttleNo();//四向穿梭车号
                                shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttleNo);
                            }

                            if (shuttleThread == null) {
                                continue;
                            }

                            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
                            if (!shuttleProtocol.isIdle()) {
                                continue;
                            }

                            wrkMast.setShuttleNo(shuttleProtocol.getShuttleNo().intValue());//给工作档分配四向穿梭车号

                            //分配任务号
                            shuttleProtocol.setTaskNo(wrkMast.getWrkNo().shortValue());
                            //分配源库位
                            shuttleProtocol.setSourceLocNo(wrkMast.getSourceLocNo());

                            //创建分配命令
                            ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                            assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());//四向穿梭车号
                            assignCommand.setTaskNo(wrkMast.getWrkNo().shortValue());//任务号
                            assignCommand.setTaskMode(ShuttleTaskModeType.PAK_IN.id.shortValue());//入出库模式
                            String currentLocNo = shuttleProtocol.getCurrentLocNo();
                            assignCommand.setSourceLocNo(currentLocNo);//源库位(小车当前位置)

                            String locNo = wrkMast.getLocNo();//当前工作档库位号
                            Integer lev = Integer.parseInt(locNo.substring(locNo.length() - 2, locNo.length()));//当前工作档库位层高
                            Integer currentLev = Integer.parseInt(currentLocNo.substring(currentLocNo.length() - 2, currentLocNo.length()));//小车当前层高

                            //获取提升机
                            LiftSlave liftSlave = slaveProperties.getLift().get(0);
                            //提升机库位号
                            String liftLocNo = liftSlave.getLiftLocNo(currentLev);

                            if (wrkMast.getWrkSts() == 8 || Boolean.parseBoolean(searchIdleShuttle.get("sameLay").toString())) {
                                //同一层直接取货无需经过提升机
                                //直接计算车到提升机取货再到库位路径指令
                                List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, liftLocNo, locNo);
                                if (wrkMast.getWrkSts() == 8) {
                                    //此时车在提升机内部，需要多下达一步指令让车移动到提升机口
                                    BasDevp basDevp = basDevpService.selectById(109);//获取提升机信息
                                    ShuttleCommand moveCommand = new ShuttleCommand();
                                    moveCommand.setCommandWord((short) 1);
                                    moveCommand.setStartCodeNum(Short.parseShort(basDevp.getQrCodeValue()));//提升机内部二维码
                                    moveCommand.setDistCodeNum(commands.get(0).getStartCodeNum());//提升机口二维码
                                    moveCommand.setStartToDistDistance(1300);
                                    moveCommand.setMiddleToDistDistance(0);
                                    moveCommand.setRunDirection(commands.get(0).getRunDirection());
                                    moveCommand.setRunSpeed((short) 1000);
                                    moveCommand.setCommandEnd((short) 1);
                                    commands.add(0, moveCommand);//将该指令添加到队头
                                }
                                assignCommand.setCommands(commands);
                                //分配目标库位
                                shuttleProtocol.setLocNo(wrkMast.getLocNo());
                                //目标库位
                                assignCommand.setLocNo(wrkMast.getLocNo());
                                wrkMast.setWrkSts(9L);//小车入库中
                            }else {
                                //不同层，将目标库位分配成提升机库位号

                                //小车移动到提升机口，计算路径
                                List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, liftLocNo, ShuttleTaskModeType.PAK_IN.id);
                                //分配目标库位
                                shuttleProtocol.setLocNo(liftLocNo);
                                //目标库位
                                assignCommand.setLocNo(liftLocNo);
                                assignCommand.setCommands(commands);
                                wrkMast.setWrkSts(5L);//小车迁移状态
                            }

                            if (wrkMastMapper.updateById(wrkMast) > 0) {
                                //下发任务
                                MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                            }
                        }

                    }
                }

            }
        }

    }

    //获取起点-终点指令。mapType：1=》无货地图，2=》有货地图
    public synchronized List<ShuttleCommand> shuttleAssignCommand(String startLocNo, String locNO,Integer mapType) {
        //计算小车起点到中点所需命令
        List<NavigateNode> calc = NavigateUtils.calc(startLocNo, locNO, mapType);
        List<ShuttleCommand> commands = new ArrayList<>();
        if (calc == null) {
            return null;
        }

        //获取分段路径
        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
        //将每一段路径分成command指令
        for (ArrayList<NavigateNode> nodes : data) {
            //开始路径
            NavigateNode startPath = nodes.get(0);
            //目标路径
            NavigateNode endPath = nodes.get(nodes.size() - 1);
            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离

            ShuttleCommand command = new ShuttleCommand();
            command.setCommandWord((short) 1);

            //通过xy坐标小车二维码
            Short startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());
            command.setStartCodeNum(startCodeNum);

            command.setMiddleCodeNum((short) 1);

            //通过xy坐标小车二维码
            Short distCodeNum = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());
            command.setDistCodeNum(distCodeNum);

            command.setStartToDistDistance(allDistance);
            command.setMiddleToDistDistance(0);
            command.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
            command.setPalletLift((short) 1);
            command.setForceMoveDistance(0);
            command.setChargeSwitch((short) 2);
            command.setIOControl((short) 0);
            command.setRunSpeed((short) 1000);
            command.setRadarTmp((short) 0);
            command.setCommandEnd((short) 1);
            commands.add(command);
        }
        return commands;
    }

    //获取起点-中点-终点指令
    public synchronized List<ShuttleCommand> shuttleAssignCommand(String startLocNo, String middleLocNo, String locNO) {
        //计算小车起点到中点所需命令
        List<NavigateNode> calc = NavigateUtils.calc(startLocNo, middleLocNo, 1);//小车无货，走入库地图
        List<ShuttleCommand> commands = new ArrayList<>();
        if (calc == null) {
            return null;
        }

        //获取分段路径
        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
        //将每一段路径分成command指令
        for (ArrayList<NavigateNode> nodes : data) {
            //开始路径
            NavigateNode startPath = nodes.get(0);
            //目标路径
            NavigateNode endPath = nodes.get(nodes.size() - 1);
            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离

            ShuttleCommand command = new ShuttleCommand();
            command.setCommandWord((short) 1);

            //通过xy坐标小车二维码
            Short startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());
            command.setStartCodeNum(startCodeNum);

            command.setMiddleCodeNum((short) 1);

            //通过xy坐标小车二维码
            Short distCodeNum = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());
            command.setDistCodeNum(distCodeNum);

            command.setStartToDistDistance(allDistance);
            command.setMiddleToDistDistance(0);
            command.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
            command.setPalletLift((short) 1);
            command.setForceMoveDistance(0);
            command.setChargeSwitch((short) 2);
            command.setIOControl((short) 0);
            command.setRunSpeed((short) 1000);
            command.setRadarTmp((short) 0);
            command.setCommandEnd((short) 1);
            commands.add(command);
        }

        //小车指令到达目标位置后，再发出一条顶升指令
        ShuttleCommand command = new ShuttleCommand();
        command.setCommandWord((short) 2);
        command.setPalletLift((short) 1);
        command.setCommandEnd((short) 1);
        commands.add(command);

        //计算小车中点到终点所需命令
        List<NavigateNode> calc2 = NavigateUtils.calc(middleLocNo, locNO, 2);//小车有货，走出库地图(出库地图有专用货道)
        if (calc2 == null) {
            return null;
        }

        //获取分段路径
        ArrayList<ArrayList<NavigateNode>> data2 = NavigateUtils.getSectionPath(calc2);
        for (ArrayList<NavigateNode> nodes : data2) {
            //开始路径
            NavigateNode startPath = nodes.get(0);
            //目标路径
            NavigateNode endPath = nodes.get(nodes.size() - 1);
            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离

            command = new ShuttleCommand();
            command.setCommandWord((short) 1);

            //通过xy坐标小车二维码
            Short startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());
            command.setStartCodeNum(startCodeNum);

            command.setMiddleCodeNum((short) 1);

            //通过xy坐标小车二维码
            Short distCodeNum = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());
            command.setDistCodeNum(distCodeNum);

            command.setStartToDistDistance(allDistance);
            command.setMiddleToDistDistance(0);
            command.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
            command.setPalletLift((short) 1);
            command.setForceMoveDistance(0);
            command.setChargeSwitch((short) 2);
            command.setIOControl((short) 0);
            command.setRunSpeed((short) 1000);
            command.setRadarTmp((short) 0);
            command.setCommandEnd((short) 1);
            commands.add(command);
        }

        //小车指令到达目标位置后，再发出一条托盘下降指令
        command = new ShuttleCommand();
        command.setCommandWord((short) 2);
        command.setPalletLift((short) 2);
        command.setCommandEnd((short) 1);
        commands.add(command);

        return commands;
    }

    /**
     * 出库  ===>>  四向穿梭车出库作业下发
     */
    public synchronized void shuttleIoOutExecute() {
        for (WrkMast wrkMast : wrkMastMapper.selectBy2125()) {
            //提取一条待出库任务
            if (wrkMast != null) {
                String outStaLocNo = null;//出库站点库位号
                //获取出库站点
                for (DevpSlave devpSlave : slaveProperties.getDevp()) {
                    for (DevpSlave.StaRack staOutRack : devpSlave.getRackOutStn()) {
                        if (staOutRack.getStaNo().equals(wrkMast.getStaNo())) {
                            //出库站点和工作档出库站点一致
                            outStaLocNo = CommonUtils.getLocNoFromRBL(staOutRack.getRow(), staOutRack.getBay(), staOutRack.getLev());
                        }
                    }

                }

                if (wrkMast.getWrkSts() == 21
                        || wrkMast.getWrkSts() == 25
                        || wrkMast.getWrkSts() == 31) {
                    ShuttleThread shuttleThread = null;
                    HashMap<String, Object> searchIdleShuttle = null;
                    if (wrkMast.getWrkSts() == 21) {
                        //寻找最近且空闲的四向穿梭车
                        searchIdleShuttle = this.searchIdleShuttle(wrkMast);
                        shuttleThread = (ShuttleThread) searchIdleShuttle.get("result");
                    }else if(wrkMast.getWrkSts() == 31 || wrkMast.getWrkSts() == 25) {
                        //继续完成之前小车未完成的任务
                        shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, wrkMast.getShuttleNo());
                    }

                    if (shuttleThread == null) {
                        continue;
                    }

                    ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
                    if (shuttleProtocol == null) {
                        continue;
                    }

                    if (!shuttleProtocol.isIdle()) {
                        continue;
                    }

                    if (outStaLocNo == null) {
                        continue;
                    }

                    wrkMast.setShuttleNo(shuttleProtocol.getShuttleNo().intValue());//给工作档分配四向穿梭车号
                    //源库位(小车当前位置)
                    String currentLocNo = shuttleProtocol.getCurrentLocNo();

                    //小车当前层高
                    Integer currentLev = Integer.parseInt(currentLocNo.substring(currentLocNo.length() - 2, currentLocNo.length()));
                    //获取提升机
                    LiftSlave liftSlave = slaveProperties.getLift().get(0);
                    //提升机库位号
                    String liftLocNo = liftSlave.getLiftLocNo(currentLev);

                    //分配任务号
                    shuttleProtocol.setTaskNo(wrkMast.getWrkNo().shortValue());
                    //分配源库位
                    shuttleProtocol.setSourceLocNo(currentLocNo);

                    ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                    //四向穿梭车号
                    assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                    //任务号
                    assignCommand.setTaskNo(wrkMast.getWrkNo().shortValue());
                    //入出库模式
                    assignCommand.setTaskMode(ShuttleTaskModeType.PAK_OUT.id.shortValue());
                    assignCommand.setSourceLocNo(currentLocNo);

                    if (wrkMast.getWrkSts() == 21) {
                        //判断小车和库位是否在同一层
                        if (Boolean.parseBoolean(searchIdleShuttle.get("sameLay").toString())) {
                            //同一层(将小车移动到货物位置)

                            List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, wrkMast.getSourceLocNo(), liftLocNo);
                            //分配目标库位
                            shuttleProtocol.setLocNo(wrkMast.getSourceLocNo());
                            //目标库位
                            assignCommand.setLocNo(wrkMast.getSourceLocNo());
                            assignCommand.setCommands(commands);
                            wrkMast.setWrkSts(26L);//小车搬运中
                        }else {
                            //不同层，将目标库位分配成提升机库位号(将小车移动到提升机位置)

                            //小车到提升机口指令
                            List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, liftLocNo, ShuttleTaskModeType.PAK_IN.id);
                            shuttleProtocol.setLocNo(liftLocNo);
                            //目标库位
                            assignCommand.setLocNo(liftLocNo);
                            assignCommand.setCommands(commands);
                            wrkMast.setWrkSts(22L);//小车迁移状态
                        }
                    } else if (wrkMast.getWrkSts() == 25) {
                        List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, wrkMast.getSourceLocNo(), liftLocNo);
                        //此时车在提升机内部，需要多下达一步指令让车移动到提升机口
                        BasDevp basDevp = basDevpService.selectById(109);//获取提升机信息
                        ShuttleCommand moveCommand = new ShuttleCommand();
                        moveCommand.setCommandWord((short) 1);//小车移动指令字
                        moveCommand.setStartCodeNum(Short.parseShort(basDevp.getQrCodeValue()));//提升机二维码
                        moveCommand.setDistCodeNum(commands.get(0).getStartCodeNum());
                        moveCommand.setStartToDistDistance(1300);
                        moveCommand.setMiddleToDistDistance(0);
                        moveCommand.setRunDirection(commands.get(0).getRunDirection());
                        moveCommand.setRunSpeed((short) 1000);
                        moveCommand.setCommandEnd((short) 1);
                        commands.add(0, moveCommand);//将该指令添加到队头

                        //分配目标库位
                        shuttleProtocol.setLocNo(wrkMast.getSourceLocNo());
                        //目标库位
                        assignCommand.setLocNo(wrkMast.getSourceLocNo());
                        assignCommand.setCommands(commands);
                        wrkMast.setWrkSts(26L);//小车搬运中
                    }

                    if (wrkMastMapper.updateById(wrkMast) > 0) {
                        //下发任务
                        MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                    }
                }
            }
        }
    }

    /**
     * 搜索空闲且最近的四向穿梭车(以工作档目标库位为基点计算最近且空闲的车)
     */
    public HashMap<String,Object> searchIdleShuttle(WrkMast wrkMast) {
        HashMap<String, Object> map = new HashMap<>();
        String locNo = wrkMast.getWrkSts() < 10 ? wrkMast.getLocNo() : wrkMast.getSourceLocNo();//库位号
        LocMast locMast = locMastService.queryByLoc(locNo);//找到库位记录
        String lay = locNo.substring(locNo.length() - 2, locNo.length());//当前工作档库位层高
        ShuttleThread recentShuttle = null;//当前距离最近的四向穿梭车线程

        for (ShuttleSlave shuttle : slaveProperties.getShuttle()) {
            //获取四向穿梭车线程
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle.getId());
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (shuttleProtocol == null) {
                continue;
            }
            if (!shuttleProtocol.isIdle()) {
                continue;
            }

            String shuttleLocNo = shuttleProtocol.getCurrentLocNo();//二维码对应库位号
            String shuttleLocNoLay = shuttleLocNo.substring(shuttleLocNo.length() - 2, shuttleLocNo.length());//库位号对应层高
            if (lay.equals(shuttleLocNoLay)) {
                //当前四向穿梭车和工作档任务在同一层，则调配该车辆
                map.put("sameLay", true);//同一层
                map.put("result", shuttleThread);
                return map;
            }

            //更新当前最近的四向穿梭车
            if (recentShuttle == null) {
                recentShuttle = shuttleThread;
            }else {
                ShuttleProtocol recentShuttleProtocol = recentShuttle.getShuttleProtocol();//目前最近穿梭车
                String recentShuttleLocNo = recentShuttleProtocol.getCurrentLocNo();//二维码对应库位号
                String recentShuttleLocNoLay = recentShuttleLocNo.substring(recentShuttleLocNo.length() - 2, recentShuttleLocNo.length());//库位号对应层高

                int recentShuttleLocNoLayInt = Integer.parseInt(recentShuttleLocNoLay);
                int layInt = Integer.parseInt(lay);
                int shuttleLocNoLayInt = Integer.parseInt(shuttleLocNoLay);
                int i = Math.abs(layInt - recentShuttleLocNoLayInt);//工作档楼层减最近穿梭车楼层，取绝对值
                int j = Math.abs(layInt - shuttleLocNoLayInt);//工作档楼层减当前穿梭车楼层，取绝对值
                if (i < j) {
                    //更新最近穿梭车
                    recentShuttle = shuttleThread;
                } else if (i == j) {
                    //楼层相同情况
                    //找距离出库点最近的车

                    if (!(recentShuttleProtocol.isIdle() && shuttleProtocol.isIdle())) {
                        //只要有一辆车不是空闲则不进行调度
                        map.put("sameLay", false);//不同层
                        map.put("result", null);
                        return map;
                    }

                    //获取提升机
                    LiftSlave liftSlave = slaveProperties.getLift().get(0);
                    //提升机库位号
                    String recentLiftLocNo = liftSlave.getLiftLocNo(recentShuttleLocNoLayInt);
                    String shuttleLiftLocNo = liftSlave.getLiftLocNo(shuttleLocNoLayInt);

                    //当前最近四向穿梭车到提升机路径
                    List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentShuttleLocNo, recentLiftLocNo, ShuttleTaskModeType.PAK_IN.id);
                    //当前楼层四向穿梭车到提升机路径
                    List<NavigateNode> shuttlePath = NavigateUtils.calc(shuttleLocNo, shuttleLiftLocNo, ShuttleTaskModeType.PAK_IN.id);
                    //判断哪一个路径最短
                    if (shuttlePath.size() < recentShuttlePath.size()) {
                        //如果当前楼层的车路径更小，则更新最近穿梭车
                        recentShuttle = shuttleThread;
                    }
                }
            }
        }

        map.put("sameLay", false);//不同层
        map.put("result", recentShuttle);
        return map;
    }

    /**
     * 四向穿梭车任务完成
     */
    public synchronized void shuttleFinished() {
        for (ShuttleSlave shuttle : slaveProperties.getShuttle()) {
            //获取四向穿梭车信息
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle.getId());
@@ -617,37 +1217,281 @@
                continue;
            }

            // 只有当四向穿梭车空闲 并且无任务时才继续执行
            if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE && shuttleProtocol.getTaskNo() == 0) {
                //入出库逻辑
                for (WrkMast wrkSts : wrkMastMapper.selectList(new EntityWrapper<WrkMast>().eq("wrk_sts", 2))) {
                    //分配任务号
                    shuttleProtocol.setTaskNo(wrkSts.getWrkNo().shortValue());
                    //分配源库位
                    shuttleProtocol.setSourceLocNo(wrkSts.getSourceLocNo());
                    //分配目标库位
                    shuttleProtocol.setLocNo(wrkSts.getLocNo());
            //四向穿梭车状态为等待确认
            if (shuttleProtocol.getProtocolStatus() == ShuttleProtocolStatusType.WAITING.id  //任务完成等待确认
                    && shuttleProtocol.getTaskNo() != 0) {
                //将任务档标记为完成
                WrkMast wrkMast = wrkMastMapper.selectByWorkNo(shuttleProtocol.getTaskNo().intValue());
                if (wrkMast != null) {
                    switch (wrkMast.getWrkSts().intValue()) {
                        case 9:
                            wrkMast.setWrkSts(14L);
                            break;
                        case 5:
                            wrkMast.setWrkSts(6L);
                            break;
                        case 22:
                            wrkMast.setWrkSts(23L);
                            break;
                        case 26:
                            wrkMast.setWrkSts(27L);
                            break;
                        default:
                    }


                    ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                    //四向穿梭车号
                    assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                    //任务号
                    assignCommand.setTaskNo(wrkSts.getWrkNo().shortValue());
                    //入出库模式
                    assignCommand.setTaskMode(0);
                    //源库位
                    assignCommand.setSourceLocNo(wrkSts.getSourceLocNo());
                    //目标库位
                    assignCommand.setDistLocNo(wrkSts.getLocNo());

                    //下发任务
                    MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                    if (wrkMastMapper.updateById(wrkMast) > 0) {
                        //设置四向穿梭车为空闲状态
                        shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
                        //任务号清零
                        shuttleProtocol.setTaskNo((short) 0);
                        //源库位清零
                        shuttleProtocol.setSourceLocNo(null);
                        //目标库位清零
                        shuttleProtocol.setLocNo(null);
                        //标记复位
                        shuttleProtocol.setPakMk(true);
                        //任务指令清零
                        shuttleProtocol.setAssignCommand(null);
                        News.info("四向穿梭车已确认且任务完成状态,复位。四向穿梭车号={}", shuttleProtocol.getShuttleNo());
                    } else {
                        News.error("四向穿梭车已确认且任务完成状态,复位失败，但未找到工作档。四向穿梭车号={}，工作号={}", shuttleProtocol.getShuttleNo(), shuttleProtocol.getTaskNo());
                    }
                }

            }
        }

            //四向穿梭车状态为充电状态
            if (shuttleProtocol.getProtocolStatus() == ShuttleProtocolStatusType.CHARGING_WAITING.id  //充电标识
                    && shuttleProtocol.getTaskNo() != 0) {
                //查询是否有充电任务
                WrkCharge wrkCharge = wrkChargeMapper.selectByWorkNo(shuttleProtocol.getTaskNo().intValue());
                if (wrkCharge != null) {
                    switch (wrkCharge.getWrkSts().intValue()) {
                        case 52://四向穿梭车迁移到提升机口
                            wrkCharge.setWrkSts(53L);//迁移完成
                            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);
                            break;
                        case 56:
                            if (shuttleProtocol.getBatteryPower() == 1000) {
                                wrkCharge.setWrkSts(60L);//充电完成
                            }
                            break;
                        default:
                    }

                    if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                        if (wrkCharge.getWrkSts() == 60) {
                            //设置四向穿梭车为空闲状态
                            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
                        }
                        //任务号清零
                        shuttleProtocol.setTaskNo((short) 0);
                        //源库位清零
                        shuttleProtocol.setSourceLocNo(null);
                        //目标库位清零
                        shuttleProtocol.setLocNo(null);
                        //标记复位
                        shuttleProtocol.setPakMk(true);
                        //任务指令清零
                        shuttleProtocol.setAssignCommand(null);
                        News.info("四向穿梭车已确认且任务完成状态,复位。四向穿梭车号={}", shuttleProtocol.getShuttleNo());
                    } else {
                        News.error("四向穿梭车已确认且任务完成状态,复位失败，但未找到工作档。四向穿梭车号={}，工作号={}", shuttleProtocol.getShuttleNo(), shuttleProtocol.getTaskNo());
                    }
                }
            }

        }
    }

    /**
     * 提升机任务
     */
    public synchronized void liftIoExecute() {
        for (LiftSlave liftSlave : slaveProperties.getLift()) {
            LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, liftSlave.getId());
            if (liftThread == null) {
                continue;
            }
            LiftProtocol liftProtocol = liftThread.getLiftProtocol();
            if (liftProtocol == null) {
                continue;
            }

            //判断提升机是否处于空闲状态
            if (!liftProtocol.isIdle()) {
                continue;
            }

            //搜索是否有待处理的任务
            WrkMast wrkMast = wrkMastMapper.selectLiftStep623();
            if (wrkMast == null) {
                continue;
            }

            //给提升机分配任务
            liftProtocol.setLiftLock(true);//锁定提升机
            liftProtocol.setTaskNo(wrkMast.getWrkNo().shortValue());//设置任务号
            liftProtocol.setShuttleNo(wrkMast.getShuttleNo().shortValue());//设置四向穿梭车号
            liftProtocol.setProtocolStatus(LiftProtocolStatusType.WORKING);//设置提升机状态为工作中

            //找到四向穿梭车的线程
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, liftProtocol.getShuttleNo().intValue());
            if (shuttleThread == null) {
                continue;
            }
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (shuttleProtocol == null) {
                continue;
            }

            //命令list
            ArrayList<LiftCommand> commands = new ArrayList<>();

            //当前穿梭车库位号
            String currentLocNo = shuttleProtocol.getCurrentLocNo();
            //当前穿梭车楼层
            int currentLocNoLey = Integer.parseInt(currentLocNo.substring(currentLocNo.length() - 2, currentLocNo.length()));

            //工作档目标库位号
            String wrkMastLocNo = wrkMast.getIoType() == 101 ? wrkMast.getSourceLocNo() : wrkMast.getLocNo();
            //工作档目标库位楼层
            int wrkMastLocNoLey = Integer.parseInt(wrkMastLocNo.substring(wrkMastLocNo.length() - 2, wrkMastLocNo.length()));

            //提升机当前楼层
            int liftLev = liftProtocol.getLev().intValue();
            if (liftLev != currentLocNoLey) {
                //不同楼层
                LiftCommand command1 = new LiftCommand();
                command1.setLiftNo(liftProtocol.getLiftNo());//提升机号
                command1.setTaskNo(liftProtocol.getTaskNo());//任务号
                command1.setRun((short) 1);//升降
                command1.setDistPosition((short) currentLocNoLey);//目标楼层(穿梭车所在楼层)
                command1.setLiftLock(true);//锁定提升机

                commands.add(command1);//将命令添加进list
            }

            //输送线将四向穿梭车移动进来
            LiftCommand command2 = new LiftCommand();
            command2.setLiftNo(liftProtocol.getLiftNo());//提升机号
            command2.setTaskNo(liftProtocol.getTaskNo());//任务号
            command2.setRun((short) 6);//输送线运作
            command2.setLiftLock(true);//锁定提升机

            commands.add(command2);//将命令添加进list

            //提升机前往目标楼层
            LiftCommand command3 = new LiftCommand();
            command3.setLiftNo(liftProtocol.getLiftNo());//提升机号
            command3.setTaskNo(liftProtocol.getTaskNo());//任务号
            command3.setRun((short) 1);//升降
            command3.setDistPosition((short) wrkMastLocNoLey);//工作档目标楼层
            command3.setLiftLock(true);//锁定提升机

            commands.add(command3);//将命令添加进list

            //提升机到达指定楼层，输送线将四向穿梭车移出去
            //输送线将四向穿梭车移动出去
            LiftCommand command4 = new LiftCommand();
            command4.setLiftNo(liftProtocol.getLiftNo());//提升机号
            command4.setTaskNo(liftProtocol.getTaskNo());//任务号
            command4.setRun((short) 3);//输送线运作
            command4.setLiftLock(true);//锁定提升机

            commands.add(command4);//将命令添加进list

            if (wrkMast.getIoType() == 101) {
                //出库任务
                wrkMast.setWrkSts(24L);//移动任务
            }else {
                //入库任务
                wrkMast.setWrkSts(7L);//移动任务
            }
            //所需命令组合完毕，更新数据库，提交到线程去工作
            LiftAssignCommand assignCommand = new LiftAssignCommand();
            assignCommand.setCommands(commands);
            assignCommand.setLiftNo(liftProtocol.getLiftNo());
            assignCommand.setTaskNo(liftProtocol.getTaskNo());
            if (wrkMastMapper.updateById(wrkMast) > 0) {
                //下发任务
                MessageQueue.offer(SlaveType.Lift, liftProtocol.getLiftNo().intValue(), new Task(3, assignCommand));
            }
        }
    }

    /**
     * 提升机任务完成
     */
    public synchronized void liftFinished() {
        for (LiftSlave liftSlave : slaveProperties.getLift()) {
            //获取提升机信息
            LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, liftSlave.getId());
            LiftProtocol liftProtocol = liftThread.getLiftProtocol();
            if (liftProtocol == null) {
                continue;
            }

            //提升机为等待确认
            if (liftProtocol.getProtocolStatus() == LiftProtocolStatusType.WAITING.id && liftProtocol.getTaskNo() != 0) {
                //将任务档标记为完成
                WrkMast wrkMast = wrkMastMapper.selectByWorkNo724(liftProtocol.getTaskNo().intValue());
                if (wrkMast != null) {
                    switch (wrkMast.getWrkSts().intValue()) {
                        case 7:
                            wrkMast.setWrkSts(8L);
                            break;
                        case 24:
                            wrkMast.setWrkSts(25L);
                            break;
                        default:
                    }

                    if (wrkMastMapper.updateById(wrkMast) > 0) {
                        //设置提升机为空闲状态
                        liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE);
                        //任务号清零
                        liftProtocol.setTaskNo((short) 0);
                        //标记复位
                        liftProtocol.setPakMk(true);
                        //任务指令清零
                        liftProtocol.setAssignCommand(null);
                        //提升机解锁
                        liftProtocol.setLiftLock(false);
                        News.info("提升机已确认且任务完成状态,复位。提升机号={}", liftProtocol.getLiftNo());
                    } else {
                        News.error("提升机已确认且任务完成状态,复位失败，但未找到工作档。提升机号={}，工作号={}", liftProtocol.getLiftNo(), liftProtocol.getTaskNo());
                    }
                }

                //查询是否有充电任务
                WrkCharge wrkCharge = wrkChargeMapper.selectByWorkNo(liftProtocol.getTaskNo().intValue());
                if (wrkCharge != null) {
                    switch (wrkCharge.getWrkSts().intValue()) {
                        case 54://提升机搬运中
                            wrkCharge.setWrkSts(55L);//提升机搬运完成
                            break;
                        default:
                    }

                    if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                        //设置提升机为空闲状态
                        liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE);
                        //任务号清零
                        liftProtocol.setTaskNo((short) 0);
                        //标记复位
                        liftProtocol.setPakMk(true);
                        //任务指令清零
                        liftProtocol.setAssignCommand(null);
                        //提升机解锁
                        liftProtocol.setLiftLock(false);
                        News.info("提升机已确认且任务完成状态,复位。提升机号={}", liftProtocol.getLiftNo());
                    } else {
                        News.error("提升机已确认且任务完成状态,复位失败，但未找到工作档。提升机号={}，工作号={}", liftProtocol.getLiftNo(), liftProtocol.getTaskNo());
                    }
                }
            }
        }
    }

    /**
@@ -2129,10 +2973,129 @@
     */
    public void recErr() {
        try {
            this.recShuttleErr();
            this.recCrnErr();
            this.recSteErr();
        } catch (Exception e) {
            News.error("recErr fail", e);
        }
    }

    /**
     * 四向穿梭车异常信息记录
     */
    private void recShuttleErr() {
        Date now = new Date();
        for (ShuttleSlave shuttleSlave : slaveProperties.getShuttle()) {
            // 获取堆垛机信息
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttleSlave.getId());
            if (shuttleThread == null) {
                continue;
            }
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (shuttleProtocol == null) {
                continue;
            }

            if (shuttleProtocol.getTaskNo() != 0) {
                //有任务
                BasShuttleErrLog latest = basShuttleErrLogService.findLatestByTaskNo(shuttleSlave.getId(), shuttleProtocol.getTaskNo().intValue());
                // 有异常
                if (latest == null) {
                    if (shuttleProtocol.getStatusErrorCode() != null && shuttleProtocol.getStatusErrorCode() > 0) {
                        WrkMast wrkMast = wrkMastMapper.selectById(shuttleProtocol.getTaskNo());
                        if (wrkMast == null) {
                            continue;
                        }
                        BasShuttleErr basShuttleErr = basShuttleErrService.queryByCode(shuttleProtocol.getStatusErrorCode().intValue());
                        String errName = basShuttleErr==null? "未知异常":basShuttleErr.getErrName();
                        BasShuttleErrLog basShuttleErrLog = new BasShuttleErrLog(
                                null,    // 编号
                                wrkMast.getWrkNo(),    // 工作号
                                now,    // 发生时间
                                null,    // 结束时间
                                wrkMast.getWrkSts(),    // 工作状态
                                wrkMast.getIoType(),    // 入出库类型
                                shuttleSlave.getId(),    // 四向穿梭车
                                null,    // plc
                                wrkMast.getLocNo(),    // 目标库位
                                wrkMast.getStaNo(),    // 目标站
                                wrkMast.getSourceStaNo(),    // 源站
                                wrkMast.getSourceLocNo(),    // 源库位
                                wrkMast.getBarcode(),    // 条码
                                (int) shuttleProtocol.getStatusErrorCode(),    // 异常码
                                errName,    // 异常
                                1,    // 异常情况
                                now,    // 添加时间
                                null,    // 添加人员
                                now,    // 修改时间
                                null,    // 修改人员
                                "任务中异常"    // 备注
                        );
                        if (!basShuttleErrLogService.insert(basShuttleErrLog)) {
                            News.error("四向穿梭车plc异常记录失败 ===>> [id:{}] [error:{}]", shuttleSlave.getId(), errName);
                        }
                    }
                } else {
                    // 异常修复
                    if (shuttleProtocol.getStatusErrorCode() == null || shuttleProtocol.getStatusErrorCode() == 0) {
                        latest.setEndTime(now);
                        latest.setUpdateTime(now);
                        latest.setStatus(2);
                        if (!basShuttleErrLogService.updateById(latest)) {
                            News.error("四向穿梭车plc异常记录修复失败 ===>> [id:{}] [errLogId:{}]", shuttleSlave.getId(), latest.getId());
                        }
                    }
                }
            }else {
                //无任务
                BasShuttleErrLog latest = basShuttleErrLogService.findLatest(shuttleSlave.getId());
                // 有异常
                if (shuttleProtocol.getStatusErrorCode() != null && shuttleProtocol.getStatusErrorCode() > 0) {
                    // 记录新异常
                    if (latest == null || (latest.getErrCode() != shuttleProtocol.getStatusErrorCode().intValue())) {
                        BasShuttleErr basShuttleErr = basShuttleErrService.queryByCode(shuttleProtocol.getStatusErrorCode().intValue());
                        String errName = basShuttleErr==null? "未知异常":basShuttleErr.getErrName();
                        BasShuttleErrLog basShuttleErrLog = new BasShuttleErrLog(
                                null,    // 编号
                                null,    // 工作号
                                now,    // 发生时间
                                null,    // 结束时间
                                null,    // 工作状态
                                null,    // 入出库类型
                                shuttleSlave.getId(),    // 四向穿梭车
                                null,    // plc
                                null,    // 目标库位
                                null,    // 目标站
                                null,    // 源站
                                null,    // 源库位
                                null,    // 条码
                                (int)shuttleProtocol.getStatusErrorCode(),    // 异常码
                                errName,    // 异常
                                1,    // 异常情况
                                now,    // 添加时间
                                null,    // 添加人员
                                now,    // 修改时间
                                null,    // 修改人员
                                "无任务异常"    // 备注
                        );
                        if (!basShuttleErrLogService.insert(basShuttleErrLog)) {
                            News.error("四向穿梭车plc异常记录失败 ===>> [id:{}] [error:{}]", shuttleSlave.getId(), errName);
                        }
                    }
                    // 无异常
                } else {
                    // 异常修复
                    if (latest != null && latest.getStatus() == 1) {
                        latest.setEndTime(now);
                        latest.setUpdateTime(now);
                        latest.setStatus(2);
                        if (!basShuttleErrLogService.updateById(latest)) {
                            News.error("四向穿梭车plc异常记录修复失败 ===>> [id:{}] [errLogId:{}]", shuttleSlave.getId(), latest.getId());
                        }
                    }
                }
            }
        }
    }

@@ -2620,6 +3583,274 @@
    }

    /**
     * 四向穿梭车电量检测 ===>> 发起充电
     */
    public synchronized void loopShuttleCharge() {
        for (DevpSlave devpSlave : slaveProperties.getDevp()) {
            SiemensDevpThread devpThread = (SiemensDevpThread) SlaveConnection.get(SlaveType.Devp, devpSlave.getId());
            for (ShuttleSlave shuttle : slaveProperties.getShuttle()) {
                //获取四向穿梭车线程
                ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle.getId());
                ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
                if (shuttleProtocol == null) {
                    continue;
                }

                //判断当前小车是否满足需要充电要求
                if (!shuttleProtocol.isRequireCharge()) {
                    continue;
                }

                WrkCharge wrkCharge = wrkChargeService.selectWorking(null, WrkChargeType.charge);
                if (wrkCharge != null) {//已有充电任务
                    continue;
                }

                ShuttleChargeType shuttleCharge = null;
                String locNo;
                for (ShuttleChargeType chargeType : ShuttleChargeType.values()) {
                    switch (chargeType.id) {
                        case 1:
                            if (devpThread.charge0 == false) {
                                ShuttleChargeType first = ShuttleChargeType.FIRST;
                                locNo = first.locNo;
                                if (wrkChargeService.selectWorkingOfCharge(first.id) == null) {
                                    shuttleCharge = first;
                                }
                            }
                            break;
                        default:
                    }
                }

                if (shuttleCharge == null) {
                    continue;
                }

                String chargeLocNo = shuttleCharge.locNo;
                wrkCharge = new WrkCharge();
                wrkCharge.setShuttleNo(shuttle.getId());
                wrkCharge.setCharge(shuttleCharge.id);
                wrkCharge.setWrkNo(commonService.getChargeWorkNo(4));
                wrkCharge.setWrkSts(51L);   // 21.准备充电
                wrkCharge.setIoPri((double) 10);
                wrkCharge.setLocNo(chargeLocNo);
                wrkCharge.setMemo("charge");
                wrkCharge.setAppeTime(new Date());
                if (!wrkChargeService.insert(wrkCharge)) {
                    News.error("保存{}号四向穿梭车充电任务失败!!!", shuttle.getId());
                    continue;
                }

                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);//充电中
                News.info("保存{}号四向穿梭车充电任务成功!!!", shuttle.getId());
            }
        }


    }

    /**
     * 执行四向穿梭车充电任务
     */
    public synchronized void executeShuttleCharge() {
        WrkCharge wrkCharge = wrkChargeService.selectWorking(null, WrkChargeType.charge);
        if (wrkCharge == null) {
            return;
        }


        ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, wrkCharge.getShuttleNo());
        if (shuttleThread == null) {
            return;
        }
        ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
        if (shuttleProtocol == null) {
            return;
        }

        //当前穿梭车库位号
        String currentLocNo = shuttleProtocol.getCurrentLocNo();
        //小车当前层高
        Integer currentLev = Integer.parseInt(currentLocNo.substring(currentLocNo.length() - 2, currentLocNo.length()));
        //获取提升机
        LiftSlave liftSlave = slaveProperties.getLift().get(0);
        //提升机库位号
        String liftLocNo = liftSlave.getLiftLocNo(currentLev);
        //充电库位号
        String chargeLocNo = wrkCharge.getLocNo();
        //充电库位层高
        Integer chargeLocNoLev = Integer.parseInt(chargeLocNo.substring(chargeLocNo.length() - 2, chargeLocNo.length()));

        if (wrkCharge.getWrkSts() == 51 || wrkCharge.getWrkSts() == 55) {
            if (currentLev == chargeLocNoLev) {
                //同一层无需经过提升机
                //直接计算车到充电库位
                //获取小车到充电库位路径指令
                List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, chargeLocNo, ShuttleTaskModeType.PAK_IN.id);
                //进行充电中
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);
                ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                assignCommand.setTaskMode((short) 9);//充电
                assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
                assignCommand.setCharge(true);//充电任务

                //创建充电指令
                ShuttleCommand command = new ShuttleCommand();
                command.setCommandWord((short) 5);//充电
                command.setShuttleNo(shuttleProtocol.getShuttleNo());
                command.setChargeSwitch((short) 1);//开始充电
                command.setCommandEnd((short) 1);
                commands.add(command);

                //指令集分配
                assignCommand.setCommands(commands);

                wrkCharge.setWrkSts(56L);//充电中状态
                if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                    //下发任务
                    MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                }
            }else {
                //不同层，将目标库位分配成提升机库位号

                //小车移动到提升机口，计算路径
                List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, liftLocNo, ShuttleTaskModeType.PAK_IN.id);
                //分配目标库位
                shuttleProtocol.setLocNo(liftLocNo);

                ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                assignCommand.setTaskMode((short) 9);//充电
                assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
                assignCommand.setCharge(true);//充电任务
                //目标库位
                assignCommand.setLocNo(liftLocNo);
                //源库位
                assignCommand.setSourceLocNo(currentLocNo);
                assignCommand.setCommands(commands);
                wrkCharge.setWrkSts(52L);//小车迁移状态
                if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                    //下发任务
                    MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                }
            }

        }else if(wrkCharge.getWrkSts() == 53){
            //小车已经达到提升机口
            LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, liftSlave.getId());
            if (liftThread == null) {
                return;
            }
            LiftProtocol liftProtocol = liftThread.getLiftProtocol();
            if (liftProtocol == null) {
                return;
            }

            //判断提升机是否处于空闲状态
            if (!liftProtocol.isIdle()) {
                return;
            }

            //给提升机分配任务
            liftProtocol.setLiftLock(true);//锁定提升机
            liftProtocol.setTaskNo(wrkCharge.getWrkNo().shortValue());//设置任务号
            liftProtocol.setShuttleNo(wrkCharge.getShuttleNo().shortValue());//设置四向穿梭车号
            liftProtocol.setProtocolStatus(LiftProtocolStatusType.WORKING);//设置提升机状态为工作中

            //命令list
            ArrayList<LiftCommand> commands = new ArrayList<>();

            //提升机当前楼层
            int liftLev = liftProtocol.getLev().intValue();
            if (liftLev != currentLev) {
                //穿梭车和提升机处于不同楼层
                LiftCommand command1 = new LiftCommand();
                command1.setLiftNo(liftProtocol.getLiftNo());//提升机号
                command1.setTaskNo(liftProtocol.getTaskNo());//任务号
                command1.setRun((short) 1);//升降
                command1.setDistPosition(currentLev.shortValue());//目标楼层(穿梭车所在楼层)
                command1.setLiftLock(true);//锁定提升机
                commands.add(command1);//将命令添加进list
            }

            //输送线将四向穿梭车移动进来
            LiftCommand command2 = new LiftCommand();
            command2.setLiftNo(liftProtocol.getLiftNo());//提升机号
            command2.setTaskNo(liftProtocol.getTaskNo());//任务号
            command2.setRun((short) 6);//输送线运作
            command2.setLiftLock(true);//锁定提升机

            commands.add(command2);//将命令添加进list

            //提升机前往目标楼层
            LiftCommand command3 = new LiftCommand();
            command3.setLiftNo(liftProtocol.getLiftNo());//提升机号
            command3.setTaskNo(liftProtocol.getTaskNo());//任务号
            command3.setRun((short) 1);//升降
            command3.setDistPosition(chargeLocNoLev.shortValue());//充电库位目标楼层
            command3.setLiftLock(true);//锁定提升机

            commands.add(command3);//将命令添加进list

            //提升机到达指定楼层，输送线将四向穿梭车移出去
            //输送线将四向穿梭车移动出去
            LiftCommand command4 = new LiftCommand();
            command4.setLiftNo(liftProtocol.getLiftNo());//提升机号
            command4.setTaskNo(liftProtocol.getTaskNo());//任务号
            command4.setRun((short) 3);//输送线运作
            command4.setLiftLock(true);//锁定提升机

            commands.add(command4);//将命令添加进list

            wrkCharge.setWrkSts(54L);//提升机搬运中
            //所需命令组合完毕，更新数据库，提交到线程去工作
            LiftAssignCommand assignCommand = new LiftAssignCommand();
            assignCommand.setCommands(commands);
            assignCommand.setLiftNo(liftProtocol.getLiftNo());
            assignCommand.setTaskNo(liftProtocol.getTaskNo());
            if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                //下发任务
                MessageQueue.offer(SlaveType.Lift, liftProtocol.getLiftNo().intValue(), new Task(3, assignCommand));
            }

        } else if (wrkCharge.getWrkSts() == 56) {
            //充电中
            //判断小车是否充满电量，满电1000或电压54V以上
            if (shuttleProtocol.getBatteryPower() >= 1000 || shuttleProtocol.getCurrentVoltage() >= 540) {
                //充满，断开充电
                List<ShuttleCommand> commands = new ArrayList<>();
                ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                assignCommand.setTaskMode((short) 0);
                assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
                assignCommand.setCharge(true);

                //创建充电指令
                ShuttleCommand command = new ShuttleCommand();
                command.setCommandWord((short) 5);//充电
                command.setShuttleNo(shuttleProtocol.getShuttleNo());
                command.setChargeSwitch((short) 2);//断开充电
                command.setCommandEnd((short) 1);
                commands.add(command);

                //指令集分配
                assignCommand.setCommands(commands);

                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING);
                //下发任务
                MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
            }

            if (shuttleProtocol.getProtocolStatus() == ShuttleProtocolStatusType.CHARGING_WAITING.id) {
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);
            }
        }

    }

    /**
     * 小车电量检测 ===>> 发起充电
     */
    @SuppressWarnings("serial")