充电任务，出入库bug

Junjie

2023-06-26 5589dd50d57175ca231827be7bb2a9fb18875c7f

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

@@ -684,13 +684,17 @@
                                //状态8，等待命令进行入库搬运动作

                                //判断提升机是否空闲
                                if (!liftProtocol.isIdle()) {
                                if (!liftProtocol.isIdleNoTask()) {
                                    try {
                                        Thread.sleep(1000);//休眠1s
                                    } catch (InterruptedException e) {
                                        throw new RuntimeException(e);
                                    }
                                    continue;//提升机忙
                                }
                                //判断提升机任务号和当前工作档任务号是否一致
                                if (liftProtocol.getTaskNo().intValue() != 0 && liftProtocol.getTaskNo().intValue() != wrkMast.getWrkNo()) {
                                    continue;
                                }

                                //判断提升机楼层是否到位，判断站点是否给出提升机到位信号
@@ -986,13 +990,17 @@
                    }else if(wrkMast.getWrkSts() == 25) {//状态25，需要向小车下发命令从提升机移动出去，需要判断提升机状是否空闲、提升机是否到达目标楼层、目标楼层站点是否存在、目标楼层站点是否给出提升机到位信号

                        //判断提升机是否空闲
                        if (!liftProtocol.isIdle()) {
                        if (!liftProtocol.isIdleNoTask()) {
                            try {
                                Thread.sleep(1000);//休眠1s
                            } catch (InterruptedException e) {
                                throw new RuntimeException(e);
                            }
                            continue;//提升机忙
                        }
                        //判断提升机任务号和当前工作档任务号是否一致
                        if (liftProtocol.getTaskNo().intValue() != 0 && liftProtocol.getTaskNo().intValue() != wrkMast.getWrkNo()) {
                            continue;
                        }

                        //判断提升机楼层是否到位，判断站点是否给出提升机到位信号
@@ -1066,6 +1074,21 @@
                                //未找到路径，等待下一次
                                continue;
                            }

                            //获取当前小车所在楼层的站点信息
                            BasDevp basDevp = basDevpService.queryByLocNo(liftSiteLocNo);
                            Short endStartCode = Short.parseShort(basDevp.getQrCodeValue());//站点二维码

                            String disLocNo = "190020" + Utils.getLev(liftSiteLocNo);//避让位置
                            LocMast locMast1 = locMastService.queryByLoc(disLocNo);
                            if (locMast1 == null) {
                                continue;//找不到库位
                            }
                            short disCode = Short.parseShort(locMast1.getQrCodeValue());
                            //任务执行完后，小车进入移开提升机口站点位置，以免坠落
                            ShuttleCommand moveCommand = shuttleThread.getMoveCommand(endStartCode, disCode, 1400, ShuttleRunDirection.BOTTOM.id, endStartCode, 1400, 500);
                            commands.add(moveCommand);

                            //分配目标库位
                            shuttleProtocol.setLocNo(wrkMast.getSourceLocNo());
                            //分配任务号
@@ -1132,10 +1155,25 @@
                        assignCommand.setTaskMode(ShuttleTaskModeType.PAK_OUT.id.shortValue());
                        assignCommand.setSourceLocNo(liftSiteLocNo);

                        //获取当前小车所在楼层的站点信息
                        BasDevp basDevp = basDevpService.queryByLocNo(liftSiteLocNo);
                        Short endStartCode = Short.parseShort(basDevp.getQrCodeValue());//站点二维码

                        String disLocNo = "190020" + Utils.getLev(liftSiteLocNo);//避让位置
                        LocMast locMast1 = locMastService.queryByLoc(disLocNo);
                        if (locMast1 == null) {
                            continue;//找不到库位
                        }
                        short disCode = Short.parseShort(locMast1.getQrCodeValue());
                        //任务执行完后，小车进入移开提升机口站点位置，以免坠落
                        ShuttleCommand moveCommand2 = shuttleThread.getMoveCommand(endStartCode, disCode, 1400, ShuttleRunDirection.BOTTOM.id, endStartCode, 1400, 500);

                        List<ShuttleCommand> commands = this.shuttleAssignCommand(liftSiteLocNo, wrkMast.getSourceLocNo(), liftSiteLocNo, assignCommand, shuttleThread);
                        if (commands == null) {
                            continue;//未找到路径
                        }
                        commands.add(moveCommand2);//任务执行完后，小车进入移开提升机口站点位置，以免坠落

                        //此时车在提升机内部，需要多下达一步指令让车移动到提升机口
                        short startCode = liftProtocol.getBarcode();//提升机内部二维码
                        Short distCode = commands.get(0).getStartCodeNum();//目标二维码
@@ -1182,7 +1220,7 @@
            //获取四向穿梭车线程
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle.getId());
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (shuttleProtocol == null) {
            if (shuttleProtocol == null || shuttleProtocol.getShuttleNo() == null) {
                continue;
            }
            if (!shuttleProtocol.isIdle()) {
@@ -1190,6 +1228,9 @@
            }

            String shuttleLocNo = shuttleProtocol.getCurrentLocNo();//二维码对应库位号
            if (shuttleLocNo == null) {
                continue;
            }
            int shuttleLocNoLey = Utils.getLev(shuttleLocNo);//库位号对应层高
            if (lev == shuttleLocNoLey) {
                //工作档楼层相同的穿梭车
@@ -1295,6 +1336,9 @@
                    && shuttleProtocol.getTaskNo() != 0
                    && shuttleProtocol.getBusyStatus() == 0
            ) {
                //标记复位
                shuttleProtocol.setPakMk(true);

                //将任务档标记为完成
                WrkMast wrkMast = wrkMastMapper.selectByWorkNo(shuttleProtocol.getTaskNo().intValue());
                if (wrkMast != null) {
@@ -1323,8 +1367,6 @@
                        shuttleProtocol.setSourceLocNo(null);
                        //目标库位清零
                        shuttleProtocol.setLocNo(null);
                        //标记复位
                        shuttleProtocol.setPakMk(true);
                        //任务指令清零
                        shuttleProtocol.setAssignCommand(null);
                        News.info("四向穿梭车已确认且任务完成状态,复位。四向穿梭车号={}", shuttleProtocol.getShuttleNo());
@@ -1346,10 +1388,8 @@
                            wrkCharge.setWrkSts(53L);//迁移完成
                            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);
                            break;
                        case 56:
                            if (shuttleProtocol.getBatteryPower() == 1000) {
                                wrkCharge.setWrkSts(60L);//充电完成
                            }
                        case 56://小车去充电桩中
                            wrkCharge.setWrkSts(57L);//到达充电桩
                            break;
                        default:
                    }
@@ -1359,16 +1399,18 @@
                            //设置四向穿梭车为空闲状态
                            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
                        }
                        //任务号清零
                        shuttleProtocol.setTaskNo((short) 0);
                        //源库位清零
                        shuttleProtocol.setSourceLocNo(null);
                        //目标库位清零
                        shuttleProtocol.setLocNo(null);
                        //标记复位
                        shuttleProtocol.setPakMk(true);
                        //任务指令清零
                        shuttleProtocol.setAssignCommand(null);
                        if (wrkCharge.getWrkSts() != 57) {
                            //任务号清零
                            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());
@@ -1393,8 +1435,13 @@
                continue;
            }

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

//            if (!liftProtocol.isIdle()) {
//                continue;
//            }

            //判断提升机是否处于空闲状态，没有判断任务号，可能提升机处于空闲，但是还有任务未完成
            if (!liftProtocol.isIdleNoTask()) {
                continue;
            }

@@ -1416,6 +1463,15 @@
            }

            if (wrkMast.getWrkSts() == 2) {//2.设备上走
                if (liftProtocol.getTaskNo().intValue() != 0) {
                    //存在未完成任务号
                    continue;
                }
                if (liftProtocol.getPlatShuttleCheck()) {
                    //提升机此时有四向车，可能有未完成的任务，禁止分配新任务
                    continue;
                }

                //工作档目标库位号
                String wrkMastLocNo = wrkMast.getLocNo();
                //工作档目标库位楼层
@@ -1460,6 +1516,10 @@

                wrkMast.setWrkSts(3L);//3.提升机搬运中
            } else if (wrkMast.getWrkSts() == 6) {//6.迁移小车至提升机口完成 => 7.提升机迁移小车中
                if (liftProtocol.getTaskNo().intValue() != 0 && liftProtocol.getTaskNo().intValue() != wrkMast.getWrkNo()) {
                    //提升机存在未完成任务，且提升机任务号和当前工作档任务号不一致
                    continue;
                }
                liftProtocol.setShuttleNo(wrkMast.getShuttleNo().shortValue());//设置四向穿梭车号

                //判断小车是否在提升机内，且处于空闲状态
@@ -1476,6 +1536,10 @@
                }
                if (shuttleProtocol.getCurrentCode().intValue() != liftProtocol.getBarcode().intValue()) {
                    continue;//小车当前二维码和提升机内部二维码不一致，不允许执行
                }
                if (!liftProtocol.getPlatShuttleCheck()) {
                    //提升机未检测到小车，禁止执行
                    continue;
                }

                //工作档目标库位号
@@ -1496,6 +1560,10 @@

                wrkMast.setWrkSts(7L);//6.迁移小车至提升机口完成 => 7.提升机迁移小车中
            } else if(wrkMast.getWrkSts() == 23) {//23.迁移小车至提升机口完成 => 24.提升机迁移小车中
                if (liftProtocol.getTaskNo().intValue() != 0 && liftProtocol.getTaskNo().intValue() != wrkMast.getWrkNo()) {
                    //提升机存在未完成任务，且提升机任务号和当前工作档任务号不一致
                    continue;
                }
                liftProtocol.setShuttleNo(wrkMast.getShuttleNo().shortValue());//设置四向穿梭车号

                //判断小车是否在提升机内，且处于空闲状态
@@ -1513,6 +1581,10 @@
                }
                if (shuttleProtocol.getCurrentCode().intValue() != liftProtocol.getBarcode().intValue()) {
                    continue;//小车当前二维码和提升机内部二维码不一致，不允许执行
                }
                if (!liftProtocol.getPlatShuttleCheck()) {
                    //提升机未检测到小车，禁止执行
                    continue;
                }

                //工作档目标库位号
@@ -1533,6 +1605,15 @@

                wrkMast.setWrkSts(24L);//23.迁移小车至提升机口完成 => 24.提升机迁移小车中
            } else if (wrkMast.getWrkSts() == 27) {//27.小车出库搬运完成
                if (liftProtocol.getTaskNo().intValue() != 0 && liftProtocol.getTaskNo().intValue() != wrkMast.getWrkNo()) {
                    //提升机存在未完成任务，且提升机任务号和当前工作档任务号不一致
                    continue;
                }
                if (liftProtocol.getPlatShuttleCheck()) {
                    //提升机此时有四向车，可能有未完成的任务，禁止分配新任务
                    continue;
                }

                //工作档源库位号
                String wrkMastLocNo = wrkMast.getSourceLocNo();
                //工作档源库位楼层
@@ -1614,6 +1695,9 @@
                    && !liftProtocol.getRunning()
            ) {

                //标记复位
                liftProtocol.setPakMk(true);

                DevpThread devpThread = null;
                Integer devpId = null;
                for (DevpSlave devp : slaveProperties.getDevp()){
@@ -1646,6 +1730,8 @@
                        case 28://28.提升机搬运中 ==> 29.提升机搬运完成
                            wrkMast.setWrkSts(29L);
                            wrkMast.setWrkSts(34L);//34.出库完成，暂时先直接完成出库工作档，后续需要根据输送线给出的状态来确定34.出库完成状态
                            //任务号清零
                            liftProtocol.setTaskNo((short) 0);
                            break;
                        default:
                    }
@@ -1653,13 +1739,9 @@
                    if (wrkMastMapper.updateById(wrkMast) > 0) {
                        //设置提升机为空闲状态
                        liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE);
                        //任务号清零
                        liftProtocol.setTaskNo((short) 0);
                        //标记复位
                        liftProtocol.setPakMk(true);
                        //任务指令清零
                        liftProtocol.setAssignCommand(null);
                        News.info("提升机已确认且任务完成状态,复位。提升机号={}", liftProtocol.getLiftNo());
                        News.info("提升机已确认且任务完成状态。提升机号={}", liftProtocol.getLiftNo());
                    } else {
                        News.error("提升机已确认且任务完成状态,复位失败，但未找到工作档。提升机号={}，工作号={}", liftProtocol.getLiftNo(), liftProtocol.getTaskNo());
                    }
@@ -2376,109 +2458,244 @@
     * 四向穿梭车电量检测 ===>> 发起充电
     */
    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;
                for (ShuttleChargeType chargeType : ShuttleChargeType.values()) {
                    if (wrkChargeService.selectWorkingOfCharge(chargeType.id) == null) {
                        shuttleCharge = chargeType;
                        break;
                    }
                }

                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());
        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(shuttleProtocol.getShuttleNo().intValue());
            if (wrkCharge != null) {//已有充电任务
                continue;
            }

            ShuttleChargeType shuttleCharge = null;
            for (ShuttleChargeType chargeType : ShuttleChargeType.values()) {
                if (wrkChargeService.selectWorkingOfCharge(chargeType.id) == null) {
                    shuttleCharge = chargeType;
                    break;
                }
            }

            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;
        }
        for (ShuttleSlave shuttle : slaveProperties.getShuttle()) {
            WrkCharge wrkCharge = wrkChargeService.selectWorking(shuttle.getId());
            if (wrkCharge == null) {
                continue;
            }

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

        //当前穿梭车库位号
        String currentLocNo = shuttleProtocol.getCurrentLocNo();
        //小车当前层高
        Integer currentLev = currentLocNo == null ? 0 : Utils.getLev(currentLocNo);
        //获取提升机
        LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1);
        if (liftThread == null) {
            return;
        }
        LiftProtocol liftProtocol = liftThread.getLiftProtocol();
        if (liftProtocol == null) {
            return;
        }
        //充电库位号
        String chargeLocNo = wrkCharge.getLocNo();
        //充电库位层高
        Integer chargeLocNoLev = Utils.getLev(chargeLocNo);
            //获取提升机
            LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1);
            if (liftThread == null) {
                continue;
            }
            LiftProtocol liftProtocol = liftThread.getLiftProtocol();
            if (liftProtocol == null) {
                continue;
            }
            //充电库位号
            String chargeLocNo = wrkCharge.getLocNo();
            //充电库位层高
            Integer chargeLocNoLev = Utils.getLev(chargeLocNo);

        if (wrkCharge.getWrkSts() == 51) {
            if (currentLev == chargeLocNoLev) {
                //同一层无需经过提升机
            if (wrkCharge.getWrkSts() == 51) {
                //当前穿梭车库位号
                String currentLocNo = shuttleProtocol.getCurrentLocNo();
                if (currentLocNo == null) {
                    continue;
                }
                //小车当前层高
                Integer currentLev = Utils.getLev(currentLocNo);

                if (currentLev == chargeLocNoLev) {
                    //同一层无需经过提升机
                    //直接计算车到充电库位

                    ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();

                    //获取小车到充电库位路径指令
                    List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, chargeLocNo, NavigationMapType.NONE.id, assignCommand, shuttleThread);
                    if (commands == null) {
                        continue;//未找到路径
                    }
                    //进行充电中
                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);

                    assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                    assignCommand.setTaskMode((short) 9);//充电
                    assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
                    assignCommand.setCharge(true);//充电任务

                    //创建充电指令
                    ShuttleCommand command = shuttleThread.getChargeSwitchCommand((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 {
                    //不同层，调度小车到充电桩目标层

                    ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();

                    //获取小车当前楼层的站点号
                    String liftSiteLocNo = Utils.levToOutInStaLocNo(currentLev);

                    //小车移动到提升机口站点，计算路径
                    List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, liftSiteLocNo, NavigationMapType.NONE.id, assignCommand, shuttleThread);
                    if (commands == null) {
                        continue;//未找到路径
                    }

                    //获取当前小车所在楼层的站点信息
                    BasDevp basDevp = basDevpService.queryByLocNo(liftSiteLocNo);
                    if (basDevp == null) {
                        continue;//找不到站点信息
                    }
                    Short basDevpQrCode = Short.parseShort(basDevp.getQrCodeValue());//站点二维码

                    //增加移动进提升机命令
                    ShuttleCommand moveCommand = shuttleThread.getMoveCommand(basDevpQrCode, liftProtocol.getBarcode(), 1400, ShuttleRunDirection.TOP.id, basDevpQrCode, 1400, 500);
                    commands.add(moveCommand);

                    //分配目标库位
                    shuttleProtocol.setLocNo(chargeLocNo);
                    assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                    assignCommand.setTaskMode((short) 9);//充电
                    assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
                    assignCommand.setCharge(true);//充电任务
                    //目标库位
                    assignCommand.setLocNo(chargeLocNo);
                    //源库位
                    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){
                //小车已经达到提升机内

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

                //判断小车是否在提升机内
                if (shuttleProtocol.getCurrentCode().intValue() != liftProtocol.getBarcode().intValue()) {
                    //小车不在提升机内
                    continue;
                }

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

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

                //提升机前往目标楼层
                //获取充电库位目标楼层命令
                LiftCommand command1 = liftThread.getLiftUpDownCommand(liftProtocol.getLiftNo(), liftProtocol.getTaskNo(), chargeLocNoLev >= 2 ? chargeLocNoLev + 1 : chargeLocNoLev);
                commands.add(command1);//将命令添加进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() == 55){//55.提升机迁移小车完成
                //直接计算车到充电库位

                ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();

                //获取小车到充电库位路径指令
                List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, chargeLocNo, NavigationMapType.NONE.id, assignCommand, shuttleThread);
                if (commands == null) {
                    return;//未找到路径
                Short liftLev = liftProtocol.getLev();
                if (liftLev == null) {
                    continue;
                }
                //判断提升机楼层是否到达目标楼层
                if (liftLev.intValue() != chargeLocNoLev) {
                    continue;//没有到达目标楼层
                }

                //此时车在提升机内部，下达一步指令让车移动到提升机口
                Integer staNo = Utils.levToOutInStaNo(liftLev >= 2 ? liftLev + 1 : liftLev);//站点号
                BasDevp basDevp = basDevpService.selectById(staNo);
                if (basDevp == null) {
                    continue;//站点不存在
                }

                //获取提升机口到充电库位路径指令
                List<ShuttleCommand> commands = this.shuttleAssignCommand(basDevp.getLocNo(), chargeLocNo, NavigationMapType.NONE.id, assignCommand, shuttleThread);
                if (commands == null) {
                    continue;//未找到路径
                }

                short startCode = liftProtocol.getBarcode();//提升机内部二维码
                Short distCode = Short.parseShort(basDevp.getQrCodeValue());//提升机口站点二维码
                Short runDirection = ShuttleRunDirection.BOTTOM.id;//运行方向
                //获取命令
                ShuttleCommand moveCommand = shuttleThread.getMoveCommand(startCode, distCode, 1400, runDirection, startCode, 1400, 500);
                commands.add(0, moveCommand);//将该指令添加到队头

                //进行充电中
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);

@@ -2499,386 +2716,40 @@
                    //下发任务
                    MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                }
            }else {
                //不同层，调度小车到充电桩目标层
            }else if (wrkCharge.getWrkSts() == 57) {//57.小车到达充电桩
                //充电中
                //判断小车是否充满电量，满电1000或电压54V以上
                if (shuttleProtocol.getBatteryPower() >= 1000 && shuttleProtocol.getCurrentVoltage() >= 54000) {
                    //充满，断开充电
                    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);

                ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                    //创建充电指令
                    ShuttleCommand command = shuttleThread.getChargeSwitchCommand((short) 2);//断开充电
                    commands.add(command);

                //获取小车当前楼层的站点号
                String liftSiteLocNo = Utils.levToOutInStaLocNo(currentLev);
                    //指令集分配
                    assignCommand.setCommands(commands);

                //小车移动到提升机口站点，计算路径
                List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, liftSiteLocNo, NavigationMapType.NONE.id, assignCommand, shuttleThread);
                if (commands == null) {
                    return;//未找到路径
                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING);

                    wrkCharge.setWrkSts(60L);//60.充电任务完成
                    if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                        //下发任务
                        MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                    }
                }

                //获取当前小车所在楼层的站点信息
                BasDevp basDevp = basDevpService.queryByLocNo(liftSiteLocNo);
                if (basDevp == null) {
                    return;//找不到站点信息
                if (shuttleProtocol.getProtocolStatus() == ShuttleProtocolStatusType.CHARGING_WAITING.id) {
                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);
                }
                Short basDevpQrCode = Short.parseShort(basDevp.getQrCodeValue());//站点二维码

                //增加移动进提升机命令
                ShuttleCommand moveCommand = shuttleThread.getMoveCommand(basDevpQrCode, liftProtocol.getBarcode(), 1400, ShuttleRunDirection.TOP.id, basDevpQrCode, 1400, 500);
                commands.add(moveCommand);

                //分配目标库位
                shuttleProtocol.setLocNo(chargeLocNo);
                assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                assignCommand.setTaskMode((short) 9);//充电
                assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
                assignCommand.setCharge(true);//充电任务
                //目标库位
                assignCommand.setLocNo(chargeLocNo);
                //源库位
                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){
            //小车已经达到提升机内

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

            //判断小车是否在提升机内
            if (shuttleProtocol.getCurrentCode().intValue() != liftProtocol.getBarcode().intValue()) {
                //小车不在提升机内
                return;
            }

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

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

            //提升机前往目标楼层
            //获取充电库位目标楼层命令
            LiftCommand command1 = liftThread.getLiftUpDownCommand(liftProtocol.getLiftNo(), liftProtocol.getTaskNo(), chargeLocNoLev >= 2 ? chargeLocNoLev + 1 : chargeLocNoLev);
            commands.add(command1);//将命令添加进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() == 55){//55.提升机迁移小车完成
            //直接计算车到充电库位

            ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();

            Short liftLev = liftProtocol.getLev();
            if (liftLev == null) {
                return;
            }
            //判断提升机楼层是否到达目标楼层
            if (liftLev.intValue() != chargeLocNoLev) {
                return;//没有到达目标楼层
            }

            //此时车在提升机内部，下达一步指令让车移动到提升机口
            Integer staNo = Utils.levToOutInStaNo(liftLev >= 2 ? liftLev + 1 : liftLev);//站点号
            BasDevp basDevp = basDevpService.selectById(staNo);
            if (basDevp == null) {
                return;//站点不存在
            }

            //获取小车到充电库位路径指令
            List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, chargeLocNo, NavigationMapType.NONE.id, assignCommand, shuttleThread);
            if (commands == null) {
                return;//未找到路径
            }

            short startCode = liftProtocol.getBarcode();//提升机内部二维码
            Short distCode = Short.parseShort(basDevp.getQrCodeValue());//提升机口站点二维码
            Short runDirection = ShuttleRunDirection.BOTTOM.id;//运行方向
            //获取命令
            ShuttleCommand moveCommand = shuttleThread.getMoveCommand(startCode, distCode, 1400, runDirection, startCode, 1400, 500);
            commands.add(0, moveCommand);//将该指令添加到队头

            //进行充电中
            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);

            assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
            assignCommand.setTaskMode((short) 9);//充电
            assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
            assignCommand.setCharge(true);//充电任务

            //创建充电指令
            ShuttleCommand command = shuttleThread.getChargeSwitchCommand((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 if (wrkCharge.getWrkSts() == 56) {
            //充电中
            //判断小车是否充满电量，满电1000或电压54V以上
            if (shuttleProtocol.getBatteryPower() >= 1000 || shuttleProtocol.getCurrentVoltage() >= 54000) {
                //充满，断开充电
                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 = shuttleThread.getChargeSwitchCommand((short) 2);//断开充电
                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);
            }
        }

    }

    /**
     * 轮询充电桩是否有空闲小车
     */
    @Deprecated
    public synchronized void queryChargeLocOfComplete() {
        // 与充电任务不同步进行
        if (null != wrkChargeService.selectWorking(null, WrkChargeType.charge)) { return; }
        if (null != wrkChargeService.selectWorking(null, WrkChargeType.reset)) { return; }
        SiemensDevpThread devpThread = (SiemensDevpThread) SlaveConnection.get(SlaveType.Devp, 1);
        // 检索充电桩
        for (SteChargeType value : SteChargeType.values()) {
            Integer steNo = basSteService.hasCarOfLocNo(value.locNo);
            if (steNo != null) {
                SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, steNo);
                SteProtocol steProtocol = steThread.getSteProtocol();
                BasSte basSte = basSteService.selectById(steNo);
                if (Cools.isEmpty(steProtocol, basSte)) {
                    continue;
                }
                if (steProtocol.getCharge() < 99) {
                    continue;
                }
                if (steProtocol.getMode() == 0) {
                    continue;
                }
                if (!steProtocol.getStatusType().equals(SteStatusType.IDLE)) {
                    continue;
                }
//                // 1号充电桩
//                if (value.equals(SteChargeType.FIRST) && devpThread.charge0) {
//                    continue;
//                }
//                // 2号充电桩
//                if (value.equals(SteChargeType.SECOND) && devpThread.charge1) {
//                    continue;
//                }
//                // 3号充电桩
//                if (value.equals(SteChargeType.THIRD) && devpThread.charge2) {
//                    continue;
//                }
//                // 小车是否处于充电状态
//                if (steProtocol.getChargeStatus() == 1) {
//                    continue;
//                }
                // case 1 : 自动充电开   馈电      ×
                // case 2 : 自动充电开   满电      ✔
                // case 3 : 自动充电关   馈电      ✔
                // case 4 : 自动充电关   满电      ✔
                if (basSte.getAutoCharge().equals("Y")
                        && steProtocol.getCharge() < Float.parseFloat(basSte.getChargeLine())) {
                    continue;
                }

                WrkCharge wrkCharge = wrkChargeService.selectWorking(steNo, WrkChargeType.reset);

                if (wrkCharge == null) {
                    // 开始穿梭车复位任务
                    wrkCharge = new WrkCharge();
                    wrkCharge.setSteNo(steNo);
                    wrkCharge.setWrkNo(commonService.getChargeWorkNo(6));
                    wrkCharge.setWrkSts(41L);   // 41.小车准备复位
                    wrkCharge.setCrnNo(2);  // 固定2号堆垛机
                    wrkCharge.setIoPri((double) 10);
                    wrkCharge.setSourceLocNo(value.locNo);
                    wrkCharge.setLocNo(basSte.getIdleLoc());
                    wrkCharge.setMemo("reset");
                    wrkCharge.setAppeTime(new Date());
                    if (!wrkChargeService.insert(wrkCharge)) {
                        News.error("保存{}号穿梭车复位任务失败!!!", steNo);
                    } else {
                        break;
                    }
                }
            }
        }
    }

    /**
     * 小车从充电桩 至 待机库位
     */
    @Deprecated
    public synchronized void steFromChargeToIdleLoc() {
        WrkCharge wrkCharge = wrkChargeService.selectWorking(null, WrkChargeType.reset);
        if (wrkCharge == null) { return; }
        SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, wrkCharge.getSteNo());
        SteProtocol steProtocol = steThread.getSteProtocol();
        BasSte basSte = basSteService.selectById(wrkCharge.getSteNo());
        if (Cools.isEmpty(steProtocol, basSte)) {
            return;
        }
        // 搬运至固定通道
        if (wrkCharge.getWrkSts() == 41L) {
            // 搬小车至小车走向通道
            List<String> channel = slaveProperties.getChannel();
            for (String channelLocNo : channel) {
                Integer otherSte = existOtherSte(channelLocNo, wrkCharge.getSteNo());
                if (null != otherSte) {
                    News.warn("{}号小车移入{}库位组失败，原因：存在{}号穿梭车！", wrkCharge.getSteNo(), channelLocNo, otherSte);
                } else {
                    // 固定堆垛机
                    int crnNo = 1;
                    if (null != wrkMastMapper.selectWorkingByCrn(crnNo)) {
                        return;
                    }

                    LocMast channelLoc = locMastService.selectById(channelLocNo);

                    CrnThread crnThread = (CrnThread) SlaveConnection.get(SlaveType.Crn, crnNo);
                    CrnProtocol crnProtocol = crnThread.getCrnProtocol();
                    if (crnProtocol == null) { continue; }
                    // 只有当堆垛机空闲 并且 无任务时才继续执行
                    if (crnProtocol.getStatusType() == CrnStatusType.IDLE && crnProtocol.getTaskNo() == 0 && crnProtocol.getModeType() == CrnModeType.AUTO) {
                        // 堆垛机命令下发区 --------------------------------------------------------------------------
                        CrnCommand crnCommand = new CrnCommand();
                        crnCommand.setCrnNo(crnNo); // 堆垛机编号
                        crnCommand.setTaskNo(wrkCharge.getWrkNo().shortValue()); // 工作号
                        crnCommand.setAckFinish((short) 0);  // 任务完成确认位
                        crnCommand.setTaskMode(CrnTaskModeType.STE_MOVE); // 任务模式:  库位移转
                        crnCommand.setSourcePosX(steProtocol.getRow());     // 源库位排
                        crnCommand.setSourcePosY(steProtocol.getBay());     // 源库位列
                        crnCommand.setSourcePosZ(steProtocol.getLev());     // 源库位层
                        crnCommand.setDestinationPosX(Utils.getGroupRow(channelLoc.getLocNo(), false).shortValue());     // 目标库位排
                        crnCommand.setDestinationPosY(channelLoc.getBay1().shortValue());     // 目标库位列
                        crnCommand.setDestinationPosZ(channelLoc.getLev1().shortValue());     // 目标库位层
                        if (!MessageQueue.offer(SlaveType.Crn, crnNo, new Task(2, crnCommand))) {
                            News.error("堆垛机命令下发失败，堆垛机号={}，任务数据={}", wrkCharge.getCrnNo(), JSON.toJSON(crnCommand));
                        } else {
                            // 修改穿梭车运行中排列层
                            steThread.modifyPos(Utils.getGroupRow(channelLoc.getLocNo(), false), channelLoc.getBay1(), channelLoc.getLev1());
                            // 修改工作档状态 41.小车准备复位 => 42.吊车搬运
                            Date now = new Date();
                            wrkCharge.setWrkSts(42L);
                            wrkCharge.setCrnStrTime(now);
                            wrkCharge.setModiTime(now);
                            if (!wrkChargeService.updateById(wrkCharge)) {
                                News.error("修改复位任务状态 41.小车准备复位 => 42.吊车搬运 失败！！，工作号={}", wrkCharge.getWrkNo());
                            }
                        }
                        break;
                    }

                }
            }
        } else if (wrkCharge.getWrkSts() == 43L) {
            // 小车行驶通道
            if (steProtocol.statusType.equals(SteStatusType.IDLE) && steProtocol.getPakMk().equals("N")) {
                // 命令下发区 --------------------------------------------------------------------------
                SteCommand steCommand = new SteCommand();
                steCommand.setSteNo(wrkCharge.getSteNo()); // 穿梭车编号
                steCommand.setTaskNo(wrkCharge.getWrkNo()); // 工作号
                steCommand.setTaskMode(SteTaskModeType.BACK_ORIGIN);  // 去左端

                steCommand.setRow(Utils.getGroupRow(steProtocol.getRow().intValue(), true).shortValue());
                steCommand.setBay(steProtocol.getBay());
                steCommand.setLev(steProtocol.getLev());
                if (!MessageQueue.offer(SlaveType.Ste, wrkCharge.getSteNo(), new Task(2, steCommand))) {
                    News.error("穿梭车命令下发失败，穿梭车号={}，任务数据={}", wrkCharge.getSteNo(), JSON.toJSON(steCommand));
                } else {
                    // 修改工作档状态 43.小车到达 ===> 44.小车走行
                    wrkCharge.setWrkSts(44L);
                    Date now = new Date();
                    wrkCharge.setCrnEndTime(now);
                    wrkCharge.setModiTime(now);
                    if (!wrkChargeService.updateById(wrkCharge)) {
                        News.error("修改复位任务状态 43.小车到达 ===> 44.小车走行 失败！！，工作号={}", wrkCharge.getWrkNo());
                    }
                }
            }
        } else if (wrkCharge.getWrkSts() == 45L) {
            if (null != wrkMastMapper.selectWorkingByCrn(wrkCharge.getCrnNo())) {
                return;
            }

            LocMast idleLoc = locMastService.selectById(basSte.getIdleLoc());

            Integer otherSte = existOtherSte(idleLoc.getLocNo(), wrkCharge.getSteNo());
            if (null != otherSte) {
                News.warn("{}号小车移入{}库位组失败，原因：存在{}号穿梭车！", wrkCharge.getSteNo(), idleLoc.getLocNo(), otherSte);
            } else {
                CrnThread crnThread = (CrnThread) SlaveConnection.get(SlaveType.Crn, wrkCharge.getCrnNo());
                CrnProtocol crnProtocol = crnThread.getCrnProtocol();
                if (crnProtocol == null) { return; }
                // 只有当堆垛机空闲 并且 无任务时才继续执行
                if (crnProtocol.getStatusType() == CrnStatusType.IDLE && crnProtocol.getTaskNo() == 0 && crnProtocol.getModeType() == CrnModeType.AUTO) {
                    // 堆垛机命令下发区 --------------------------------------------------------------------------
                    CrnCommand crnCommand = new CrnCommand();
                    crnCommand.setCrnNo(wrkCharge.getCrnNo()); // 堆垛机编号
                    crnCommand.setTaskNo(wrkCharge.getWrkNo().shortValue()); // 工作号
                    crnCommand.setAckFinish((short) 0);  // 任务完成确认位
                    crnCommand.setTaskMode(CrnTaskModeType.STE_MOVE); // 任务模式:  库位移转
                    crnCommand.setSourcePosX(Utils.getGroupRow(steProtocol.getRow().intValue(), true).shortValue());     // 源库位排
                    crnCommand.setSourcePosY(steProtocol.getBay());     // 源库位列
                    crnCommand.setSourcePosZ(steProtocol.getLev());     // 源库位层
                    crnCommand.setDestinationPosX(Utils.getGroupRow(idleLoc.getLocNo(), true).shortValue());     // 目标库位排
                    crnCommand.setDestinationPosY(idleLoc.getBay1().shortValue());     // 目标库位列
                    crnCommand.setDestinationPosZ(idleLoc.getLev1().shortValue());     // 目标库位层
                    if (!MessageQueue.offer(SlaveType.Crn, wrkCharge.getCrnNo(), new Task(2, crnCommand))) {
                        News.error("堆垛机命令下发失败，堆垛机号={}，任务数据={}", wrkCharge.getCrnNo(), JSON.toJSON(crnCommand));
                    } else {
                        // 修改穿梭车运行中排列层
                        steThread.modifyPos(Utils.getGroupRow(idleLoc.getLocNo(), true), idleLoc.getBay1(), idleLoc.getLev1());
                        // 修改工作档状态 45.小车待搬 => 46.放至待机位
                        Date now = new Date();
                        wrkCharge.setWrkSts(46L);
                        wrkCharge.setCrnStrTime(now);
                        wrkCharge.setModiTime(now);
                        if (!wrkChargeService.updateById(wrkCharge)) {
                            News.error("修改工作档状态 45.小车待搬 => 46.放至待机位 失败！！，工作号={}", wrkCharge.getWrkNo());
                        }
                    }
                }

            }
        }
    }

    public List<String> crn2DemoLocs = new ArrayList<String>(); public String crn2LastLoc = "";