充电

Junjie

2023-08-01 d7b48f2e68180b36ca39c6675b2a861601c831df

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

@@ -2,10 +2,7 @@

import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.mapper.EntityWrapper;
import com.baomidou.mybatisplus.mapper.Wrapper;
import com.core.common.Cools;
import com.core.common.SpringUtils;
import com.core.exception.CoolException;
import com.zy.asrs.entity.*;
import com.zy.asrs.mapper.*;
@@ -13,11 +10,9 @@
import com.zy.asrs.utils.Utils;
import com.zy.common.model.*;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.common.model.enums.WrkChargeType;
import com.zy.common.service.CommonService;
import com.zy.common.service.erp.ErpService;
import com.zy.common.utils.*;
import com.zy.core.CrnThread;
import com.zy.core.DevpThread;
import com.zy.core.News;
import com.zy.core.cache.MessageQueue;
@@ -728,7 +723,7 @@
            //判断小车是否到达输送站点库位
            if (!shuttleProtocol.getCurrentLocNo().equals(basDevp.getLocNo())) {
                //小车不在输送站点位置
                dispatchShuttle(wrkMast.getWrkNo(), basDevp.getLocNo());//调度小车到货物所在输送站点进行取货
                dispatchShuttle(wrkMast.getWrkNo(), basDevp.getLocNo(), wrkMast.getShuttleNo());//调度小车到货物所在输送站点进行取货
                return false;
            }

@@ -959,235 +954,76 @@
     */
    public synchronized void shuttleOutExecute() {
        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;

                    LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1);
                    LiftProtocol liftProtocol = liftThread.getLiftProtocol();

                    if (wrkMast.getWrkSts() == 21) {
                        if (wrkMast.getShuttleNo() == null) {
                            //寻找最近且空闲的四向穿梭车
                            searchIdleShuttle = this.searchIdleShuttle(wrkMast);
                            shuttleThread = (ShuttleThread) searchIdleShuttle.get("result");
                            if (shuttleThread == null) {
                                continue;
                            }
                            wrkMast.setShuttleNo(shuttleThread.getSlave().getId());//给工作档分配四向穿梭车号
                            wrkMastMapper.updateById(wrkMast);
                        }else {
                            //直接使用任务保存中的小车
                            shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, wrkMast.getShuttleNo());
                        }
                    }else if(wrkMast.getWrkSts() == 25) {//状态25，需要向小车下发命令从提升机移动出去，需要判断提升机状是否空闲、提升机是否到达目标楼层、目标楼层站点是否存在、目标楼层站点是否给出提升机到位信号

                        //判断提升机是否空闲
                        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;
                        }

                        //判断提升机楼层是否到位，判断站点是否给出提升机到位信号
                        String locNo = wrkMast.getSourceLocNo();
                        int lev = Utils.getLev(locNo);//目标二维码所在楼层
                        Short liftLev = liftProtocol.getLev();//提升机所在楼层
                        if (liftLev == null) {//提升机可能在输送线楼层
                            continue;
                        }
                        if (liftLev.intValue() != lev) {
                            continue;//提升机不在目标楼层跳过
                        }

                        Integer staNo = Utils.levToOutInStaNo(lev >= 2 ? lev + 1 : lev);
                        DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, 1);
                        //获取目标站信息
                        StaProtocol staProtocol1 = devpThread.getStation().get(staNo);
                        if (staProtocol1 == null) {
                            continue;//站点信息不存在
                        }
                        if (!staProtocol1.isLiftArrival()) {
                            continue;//站点提升机到位信号false
                        }

                        //继续完成之前小车未完成的任务
                        shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, wrkMast.getShuttleNo());
                    } else if (wrkMast.getWrkSts() == 31) {
                        //继续完成之前小车未完成的任务
                        shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, wrkMast.getShuttleNo());
                    }

                    if (shuttleThread == null) {
                        continue;
                    }

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

                    if (outStaLocNo == null) {
                        continue;
                    }

                    if (wrkMast.getWrkSts() == 21) {

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

                        //源库位(小车当前位置)
                        String currentLocNo = shuttleProtocol.getCurrentLocNo();

                        //小车当前层高
                        Integer currentLev = Utils.getLev(currentLocNo);
                        //当前楼层提升机输送站点库位号
                        String liftSiteLocNo = Utils.levToOutInStaLocNo(currentLev);

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

                        //判断小车和库位是否在同一层
                        if (currentLev == Utils.getLev(wrkMast.getSourceLocNo())) {
                            //同一层(将小车移动到货物位置)

                            List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, wrkMast.getSourceLocNo(), liftSiteLocNo, assignCommand, shuttleThread);
                            if (commands == null) {
                                //未找到路径，等待下一次
                                continue;
                            }

                            //分配目标库位
                            shuttleProtocol.setLocNo(wrkMast.getSourceLocNo());
                            //分配任务号
                            shuttleProtocol.setTaskNo(wrkMast.getWrkNo().shortValue());
                            //分配源库位
                            shuttleProtocol.setSourceLocNo(currentLocNo);
                            //目标库位
                            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));
                            }
                        }else {
                            //不同层，将目标库位分配成提升机库位号(将小车移动到提升机位置)

                            //小车到提升机口指令
                            List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, liftSiteLocNo, ShuttleTaskModeType.PAK_IN.id, assignCommand, shuttleThread);
                            if (commands == null) {
                                if (!currentLocNo.equals(liftSiteLocNo)) {//当前位置也不在提升机口
                                    continue;//未找到路径
                                }
                                commands = new ArrayList<>();
                            }
                            shuttleProtocol.setLocNo(liftSiteLocNo);
                            //分配任务号
                            shuttleProtocol.setTaskNo(wrkMast.getWrkNo().shortValue());
                            //分配源库位
                            shuttleProtocol.setSourceLocNo(currentLocNo);

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

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

                            //目标库位
                            assignCommand.setLocNo(liftSiteLocNo);
//                            assignCommand.setCommands(commands);
                            wrkMast.setWrkSts(22L);//小车迁移状态

                            if (wrkMastMapper.updateById(wrkMast) > 0) {
                                //下发任务
                                MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                            }
                        }
                    } else if (wrkMast.getWrkSts() == 25) {
                        if (!shuttleProtocol.isIdle(wrkMast.getWrkNo().shortValue())) {
                            continue;
                        }

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

                        //当前楼层提升机输送站点库位号
                        String liftSiteLocNo = Utils.levToOutInStaLocNo(liftProtocol.getLev().intValue());

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

                        List<ShuttleCommand> commands = this.shuttleAssignCommand(liftSiteLocNo, wrkMast.getSourceLocNo(), liftSiteLocNo, assignCommand, shuttleThread);
                        if (commands == null) {
                            continue;//未找到路径
                        }

//                        //此时车在提升机内部，需要多下达一步指令让车移动到提升机口
//                        short startCode = liftProtocol.getBarcode();//提升机内部二维码
//                        Short distCode = commands.get(0).getStartCodeNum();//目标二维码
//                        //获取移动命令
//                        ShuttleCommand moveCommand = shuttleThread.getMoveCommand(startCode, distCode, 1600, commands.get(0).getRunDirection(), null, null, 500);
//                        commands.add(0, moveCommand);//将该指令添加到队头

                        //分配目标库位
                        shuttleProtocol.setLocNo(wrkMast.getSourceLocNo());
                        //分配任务号
                        shuttleProtocol.setTaskNo(wrkMast.getWrkNo().shortValue());
                        //分配源库位
                        shuttleProtocol.setSourceLocNo(liftSiteLocNo);
                        //目标库位
                        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));
                        }
                    }

                }
            boolean step1 = this.shuttleOutExecuteStep1(wrkMast);//小车搬出库中
            if (!step1) {
                continue;
            }
        }
    }

    /**
     * 出库-小车搬出库中
     * 如需主方法执行continue，请返回false
     * ps:返回值true并不代表该方法执行成功，返回值仅做标记用于主方法是否执行continue
     */
    public boolean shuttleOutExecuteStep1(WrkMast wrkMast) {
        //21.生成出库任务 => 22.小车搬运中
        if (wrkMast.getWrkSts() == 21) {
            if (wrkMast.getShuttleNo() == null) {//没有绑定小车，进行调度
                dispatchShuttle(wrkMast.getWrkNo(), wrkMast.getSourceLocNo());//调度小车到货物所在库位进行取货
                return false;
            }

            //获取四向穿梭车线程
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, wrkMast.getShuttleNo());
            if (shuttleThread == null) {
                return false;
            }
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (shuttleProtocol == null) {
                return false;
            }
            if (!shuttleProtocol.isIdle()) {
                return false;
            }

            //获取目标站对应的输送站点
            BasDevp targetBasDevp = basDevpService.selectByLevAndLiftNo(Utils.getLev(wrkMast.getSourceLocNo()), wrkMast.getLiftNo());
            if (targetBasDevp == null) {
                return false;//缺少站点信息
            }

            //判断小车是否到达货物库位
            if (!shuttleProtocol.getCurrentLocNo().equals(wrkMast.getSourceLocNo())) {
                //小车不在输送站点位置
                dispatchShuttle(wrkMast.getWrkNo(), wrkMast.getSourceLocNo(), wrkMast.getShuttleNo());//调度小车到货物所在输送站点进行取货
                return false;
            }

            //小车已抵达货物位置，进行搬运货物
            NyShuttleOperaResult result = NyShuttleOperaUtils.getShuttleTransportCommands(wrkMast.getShuttleNo(), wrkMast.getWrkNo(), shuttleProtocol.getCurrentLocNo(), wrkMast.getSourceLocNo(), targetBasDevp.getLocNo());
            if (result == null) {//出库路径计算失败
                return false;
            }

            //创建分配命令
            ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
            assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());//四向穿梭车号
            assignCommand.setTaskNo(wrkMast.getWrkNo().shortValue());//任务号
            assignCommand.setTaskMode(ShuttleTaskModeType.PAK_OUT.id.shortValue());//出库模式
            assignCommand.setSourceLocNo(shuttleProtocol.getCurrentLocNo());//源库位(小车当前位置)
            assignCommand.setCommands(result.getCommands());//运行命令
            assignCommand.setNodes(result.getNodes());//路径节点

            wrkMast.setWrkSts(22L);//21.生成出库任务 => 22.小车搬运中
            wrkMast.setModiTime(new Date());
            if (wrkMastMapper.updateById(wrkMast) > 0) {
                //下发任务
                MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
            }
            return false;
        }
        return true;
    }

    /**
@@ -1322,8 +1158,8 @@
    public synchronized void shuttleFinished() {
        for (ShuttleSlave shuttle : slaveProperties.getShuttle()) {
            //获取四向穿梭车信息
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle.getId());
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            NyShuttleThread shuttleThread = (NyShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle.getId());
            NyShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (shuttleProtocol == null) {
                continue;
            }
@@ -1331,7 +1167,7 @@
            //四向穿梭车状态为等待确认、小车处于空闲状态
            if (shuttleProtocol.getProtocolStatus() == ShuttleProtocolStatusType.WAITING.id  //任务完成等待确认
                    && shuttleProtocol.getTaskNo() != 0
                    && shuttleProtocol.getBusyStatus() == 0
                    && shuttleProtocol.getFree() == ShuttleStatusType.IDLE.id
            ) {
                //将任务档标记为完成
                WrkMast wrkMast = wrkMastMapper.selectByWorkNo(shuttleProtocol.getTaskNo().intValue());
@@ -1340,7 +1176,7 @@
                        case 5://5.小车搬运中 ==> 9.入库完成
                            wrkMast.setWrkSts(9L);
                            //任务号清零
                            shuttleProtocol.setTaskNo((short) 0);
                            shuttleProtocol.setTaskNo(0);
                            break;
                        case 22://22.小车搬运中 ==> 23.小车搬运完成
                            wrkMast.setWrkSts(23L);
@@ -2211,8 +2047,8 @@
    public synchronized void loopShuttleCharge() {
        for (ShuttleSlave shuttle : slaveProperties.getShuttle()) {
            //获取四向穿梭车线程
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle.getId());
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            NyShuttleThread shuttleThread = (NyShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle.getId());
            NyShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (shuttleProtocol == null) {
                continue;
            }
@@ -2269,250 +2105,78 @@
                continue;
            }

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

            //获取提升机
            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) {
                //当前穿梭车库位号
                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(ShuttleTaskModeType.CHARGE.id.shortValue());//充电
                    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(), 1600, ShuttleRunDirection.TOP.id, basDevpQrCode, 1600, 500);
//                    commands.add(moveCommand);

                    //分配目标库位
                    shuttleProtocol.setLocNo(chargeLocNo);
                    assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                    assignCommand.setTaskMode(ShuttleTaskModeType.CHARGE.id.shortValue());//充电
                    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(wrkCharge.getWrkNo().shortValue())) {
                if (!shuttleProtocol.getCurrentLocNo().equals(wrkCharge.getLocNo())) {
                    //小车不在充电桩位置
                    dispatchShuttle(wrkCharge.getWrkNo(), wrkCharge.getLocNo(), shuttle.getId());//调度小车去充电桩
                    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.提升机迁移小车完成
                //直接计算车到充电库位

                //小车已经在充电桩位置，下发充电命令
                NyShuttleHttpCommand chargeCommand = NyHttpUtils.getChargeCommand(shuttle.getId(), wrkCharge.getWrkNo(), true);
                ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
                commands.add(chargeCommand);
                //创建分配命令
                ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());//四向穿梭车号
                assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());//任务号
                assignCommand.setTaskMode(ShuttleTaskModeType.CHARGE.id.shortValue());//出库模式
                assignCommand.setSourceLocNo(shuttleProtocol.getCurrentLocNo());//源库位(小车当前位置)
                assignCommand.setCommands(commands);//运行命令

                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, 1600, runDirection, startCode, 1600, 500);
//                commands.add(0, moveCommand);//将该指令添加到队头

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

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

                //创建充电指令
                ShuttleCommand command = shuttleThread.getChargeSwitchCommand((short) 1);//开始充电
                commands.add(command);

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

                wrkCharge.setWrkSts(56L);//充电中状态
                wrkCharge.setWrkSts(52L);//51.生成充电任务 => 52.小车去充电中
                wrkCharge.setModiTime(new Date());
                if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                    //下发任务
                    MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                }
            }else if (wrkCharge.getWrkSts() == 57) {//57.小车到达充电桩
                //充电中
                //判断小车是否充满电量，满电1000或电压54V以上
                if (shuttleProtocol.getBatteryPower() == null || shuttleProtocol.getCurrentVoltage() == null) {
            } else if (wrkCharge.getWrkSts() == 52) {
                //检测小车是否满电
                if (shuttleProtocol.getPowerPercent() < 100) {
                    continue;
                }
                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);

                    //将小车移动到空闲的巷道
                    ShuttleAssignCommand assignCommand = Utils.searchEmptyGroupToMoveShuttle(Utils.getLev(shuttleProtocol.getLocNo()), shuttleProtocol.getShuttleNo().intValue(), shuttleThread, null);
                    if (assignCommand == null) {
                        continue;
                    }
                //小车满电，结束充电任务
                NyShuttleHttpCommand chargeCommand = NyHttpUtils.getChargeCommand(shuttle.getId(), wrkCharge.getWrkNo(), false);
                ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
                commands.add(chargeCommand);
                //创建分配命令
                ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());//四向穿梭车号
                assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());//任务号
                assignCommand.setTaskMode(ShuttleTaskModeType.CHARGE.id.shortValue());//出库模式
                assignCommand.setSourceLocNo(shuttleProtocol.getCurrentLocNo());//源库位(小车当前位置)
                assignCommand.setCommands(commands);//运行命令

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

                if (shuttleProtocol.getProtocolStatus() == ShuttleProtocolStatusType.CHARGING_WAITING.id) {
                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING);
                wrkCharge.setWrkSts(60L);//52.小车去充电中 => 60.充电任务完成
                wrkCharge.setModiTime(new Date());
                if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                    //下发任务
                    MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                }
            }

        }

    }

    /**
     * 调度车辆-调度指定穿梭车
     */
    public void dispatchShuttle(Integer wrkNo, String locNo, Integer shuttleNo) {
        shuttleMoveGenerate(wrkNo, locNo, shuttleNo);
    }

    /**
     * 调度车辆
     */
    public void dispatchShuttle(Integer wrkNo, String locNo) {