牛眼坐标与WCS系统坐标转换公式

Junjie

2023-07-29 c60cfa9037592af51f47cccf1c063ebc64923394

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

@@ -3,6 +3,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;
@@ -200,16 +201,17 @@
                    }

                    // 过滤盘点/拣料/并板任务
//                    if (null != wrkMastMapper.selectPickStepByBarcode(barcode)) {
//                        continue;
//                    }
                    WrkMast wrkMast1 = wrkMastMapper.selectPickStepByBarcode(barcode);
                    if (null != wrkMast1) {
                        continue;
                    }

                    // 判断重复工作档
//                    WrkMast wrkMast = wrkMastMapper.selectPakInStep1(inSta.getStaNo(), barcode);
//                    if (wrkMast != null) {
//                        News.error("工作档中已存在该站状态为（ 2.设备上走 ）的数据,工作号={}", wrkMast.getWrkNo());
//                        continue;
//                    }
                    WrkMast wrkMast2 = wrkMastMapper.selectPakInStep1(inSta.getStaNo(), barcode);
                    if (wrkMast2 != null) {
                        News.error("工作档中已存在该站状态为（ 2.设备上走 ）的数据,工作号={}", wrkMast2.getWrkNo());
                        continue;
                    }

                    try {
                        LocTypeDto locTypeDto = new LocTypeDto(staProtocol);
@@ -382,6 +384,7 @@
    /**
     * 拣料、并板、盘点再入库
     */
    @Transactional
    public synchronized void stnToCrnStnPick(){
        for (DevpSlave devp : slaveProperties.getDevp()) {
            // 遍历拣料入库口
@@ -397,25 +400,20 @@
                }
                if (staProtocol.isAutoing()
                        && staProtocol.isLoading()
                        && staProtocol.isInEnable()
                        // 0 - 9990 或者 9996
                        && (staProtocol.getWorkNo() > 0 && staProtocol.getWorkNo() < 9990)
                        && (staProtocol.getWorkNo() > 0 && staProtocol.getWorkNo() < 9999)
                        && staProtocol.getStaNo().equals(staProtocol.getSiteId().shortValue())
                        && staProtocol.isPakMk()){

                    // 获取条码扫描仪信息
//                    BarcodeThread barcodeThread = (BarcodeThread) SlaveConnection.get(SlaveType.Barcode, pickSta.getBarcode());
//                    if (barcodeThread == null) {
//                        continue;
//                    }
                    BarcodeThread barcodeThread = (BarcodeThread) SlaveConnection.get(SlaveType.Barcode, pickSta.getBarcode());
                    if (barcodeThread == null) {
                        continue;
                    }
                    String barcode = barcodeThread.getBarcode();

                    WrkMast wrkMast = null;
                    if (staProtocol.getWorkNo() > 0 && staProtocol.getWorkNo() < 9990) {
                        wrkMast = wrkMastMapper.selectPickStep(staProtocol.getWorkNo());
                        if (null == wrkMast) {
                            continue;
                        }
                    } else {
                    wrkMast = wrkMastMapper.selectPickStepByBarcode(barcode);
                    if (wrkMast == null) {
                        continue;
                    }
//                    if (staProtocol.getWorkNo() == 9996) {
@@ -455,6 +453,7 @@
//                            News.error("{}条码错误，暂无拣料任务！", barcode);
//                        }
//                    }

                    if ((wrkMast.getIoType() != 103 && wrkMast.getIoType() != 104 && wrkMast.getIoType() != 107)
                        || Cools.isEmpty(wrkMast.getStaNo()) || Cools.isEmpty(wrkMast.getSourceStaNo()) ) {
                        continue;
@@ -509,14 +508,22 @@
                            wrkMast.setSourceStaNo(pickSta.getStaNo()); // 源站
                            wrkMast.setStaNo(dto.getStaNo()); // 目标站
                            wrkMast.setCrnNo(dto.getCrnNo());
                            wrkMast.setLocNo(dto.getLocNo()); // 目标库位 = 出库时的源库位
                            wrkMast.setSourceLocNo(""); // 源库位清空
                            wrkMast.setLocNo(sourceLocNo); // 目标库位 = 出库时的源库位
                            wrkMast.setShuttleNo(null); // 穿梭车清空
                            wrkMast.setModiTime(new Date());
                            if (wrkMastMapper.updateById(wrkMast) == 0) {
                                throw new CoolException(wrkMast.getWrkNo() + "更新工作档数据状态失败");
                            }
                            if (wrkMastMapper.setSteEmpty(wrkMast.getWrkNo()) == 0) {
                                throw new CoolException(wrkMast.getWrkNo() + "更新工作档数据状态失败");
                            }

                            // 修改库位状态 Q.拣料/盘点/并板再入库
                            LocMast locMast = locMastService.selectById(sourceLocNo);
                            locMast.setLocSts("Q");
                            locMast.setModiTime(new Date());
                            if (!locMastService.updateById(locMast)) {
                                throw new CoolException("修改库位状态失败");
                            }

                            // 更新站点信息 且 下发plc命令
@@ -565,7 +572,7 @@
     * 初始化实时地图
     */
    public synchronized void initRealtimeBasMap() {
        for (int i = 1; i <= 4; i++) {//总共四层楼
        for (int i = 1; i <= 10; i++) {//总共四层楼
            Object data = redisUtil.get("realtimeBasMap_" + i);
            if (data == null) {//redis地图数据为空，从数据库中获取
                BasMap basMap = basMapService.selectLatestMap(i);
@@ -778,7 +785,7 @@
                                    if (commands == null) {
                                        continue;//找不到路径等待下一次
                                    }
                                    assignCommand.setCommands(commands);
//                                    assignCommand.setCommands(commands);
                                    //分配目标库位
                                    shuttleProtocol.setLocNo(wrkMast.getLocNo());
                                    //目标库位
@@ -804,7 +811,7 @@
                                    shuttleProtocol.setLocNo(liftSiteLocNo);
                                    //目标库位
                                    assignCommand.setLocNo(liftSiteLocNo);
                                    assignCommand.setCommands(commands);
//                                    assignCommand.setCommands(commands);
                                    wrkMast.setWrkSts(5L);//小车迁移状态
                                }
                            } else if (wrkMast.getWrkSts() == 8) {
@@ -825,7 +832,7 @@
                                ShuttleCommand moveCommand = shuttleThread.getMoveCommand(startCode, distCode, 1600, runDirection, null, null, 500);
                                commands.add(0, moveCommand);//将该指令添加到队头

                                assignCommand.setCommands(commands);
//                                assignCommand.setCommands(commands);
                                //分配目标库位
                                shuttleProtocol.setLocNo(wrkMast.getLocNo());
                                //目标库位
@@ -902,12 +909,12 @@
        //获取小车移动速度
        BasShuttle basShuttle = basShuttleService.selectById(assignCommand.getShuttleNo());
        Integer runSpeed = 1000;
        if (basShuttle != null) {
            Integer runSpeed1 = basShuttle.getRunSpeed();
            if (runSpeed1 != null) {
                runSpeed = runSpeed1;
            }
        }
//        if (basShuttle != null) {
//            Integer runSpeed1 = basShuttle.getRunSpeed();
//            if (runSpeed1 != null) {
//                runSpeed = runSpeed1;
//            }
//        }

        //计算小车起点到中点所需命令
        List<NavigateNode> calc = NavigateUtils.calc(startLocNo, locNo, mapType, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), Utils.getLev(startLocNo)));
@@ -938,13 +945,13 @@
                middlePath = nodes.get(nodes.size() - 3);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, 3);//计算中间点到目标点行走距离
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            } else if (nodes.size() > 5) {//中段码传倒数第二个
                //中间路径
                middlePath = nodes.get(nodes.size() - 2);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, 2);//计算中间点到目标点行走距离
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            }

            //目标路径
@@ -972,12 +979,12 @@
        //获取小车移动速度
        BasShuttle basShuttle = basShuttleService.selectById(assignCommand.getShuttleNo());
        Integer runSpeed = 1000;
        if (basShuttle != null) {
            Integer runSpeed1 = basShuttle.getRunSpeed();
            if (runSpeed1 != null) {
                runSpeed = runSpeed1;
            }
        }
//        if (basShuttle != null) {
//            Integer runSpeed1 = basShuttle.getRunSpeed();
//            if (runSpeed1 != null) {
//                runSpeed = runSpeed1;
//            }
//        }

        List<NavigateNode> allNode = new ArrayList<>();

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

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

            //目标路径
@@ -1233,7 +1240,7 @@
                            shuttleProtocol.setSourceLocNo(currentLocNo);
                            //目标库位
                            assignCommand.setLocNo(wrkMast.getSourceLocNo());
                            assignCommand.setCommands(commands);
//                            assignCommand.setCommands(commands);
                            wrkMast.setWrkSts(26L);//小车搬运中

                            if (wrkMastMapper.updateById(wrkMast) > 0) {
@@ -1267,7 +1274,7 @@

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

                            if (wrkMastMapper.updateById(wrkMast) > 0) {
@@ -1314,7 +1321,7 @@
                        shuttleProtocol.setSourceLocNo(liftSiteLocNo);
                        //目标库位
                        assignCommand.setLocNo(wrkMast.getSourceLocNo());
                        assignCommand.setCommands(commands);
//                        assignCommand.setCommands(commands);
                        wrkMast.setWrkSts(26L);//小车搬运中

                        if (wrkMastMapper.updateById(wrkMast) > 0) {
@@ -1343,10 +1350,10 @@

        //判断其他空闲穿梭车是否离任务最近
        String distLocNo = null;//目标地点，入库=》提升机口，出库=》货物库位号
        if (wrkMast.getIoType() == 1) {
        if (wrkMast.getIoType() < 101 && wrkMast.getIoType() != 11) {
            //入库
            distLocNo = Utils.levToOutInStaLocNo(lev);
        }else if(wrkMast.getIoType() == 101){
        }else if(wrkMast.getIoType() >= 101){
            //出库
            distLocNo = locNo;
        } else if (wrkMast.getIoType() == 11) {
@@ -1865,7 +1872,7 @@

                //所需命令组合完毕，更新数据库，提交到线程去工作
                LiftAssignCommand assignCommand = new LiftAssignCommand();
                assignCommand.setCommands(commands);
//                assignCommand.setCommands(commands);
                assignCommand.setLiftNo(liftProtocol.getLiftNo());
                assignCommand.setTaskNo(liftProtocol.getTaskNo());
                if (wrkMastMapper.updateById(wrkMast) > 0) {
@@ -2119,7 +2126,7 @@
                shuttleProtocol.setLocNo(liftSiteLocNo);
                //目标库位
                assignCommand.setLocNo(liftSiteLocNo);
                assignCommand.setCommands(commands);
//                assignCommand.setCommands(commands);
                wrkMast.setWrkSts(5L);//小车迁移状态

                if (wrkMastMapper.updateById(wrkMast) > 0) {
@@ -2226,7 +2233,7 @@
            //分配源库位
            shuttleProtocol.setSourceLocNo(wrkMast.getSourceLocNo());

            assignCommand.setCommands(commands);
//            assignCommand.setCommands(commands);
            //分配目标库位
            shuttleProtocol.setLocNo(wrkMast.getLocNo());
            //目标库位
@@ -2744,7 +2751,7 @@
                    commands.add(command);

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

                    wrkCharge.setWrkSts(56L);//充电中状态
                    if (wrkChargeMapper.updateById(wrkCharge) > 0) {
@@ -2786,7 +2793,7 @@
                    assignCommand.setLocNo(chargeLocNo);
                    //源库位
                    assignCommand.setSourceLocNo(currentLocNo);
                    assignCommand.setCommands(commands);
//                    assignCommand.setCommands(commands);
                    wrkCharge.setWrkSts(52L);//小车迁移状态
                    if (wrkChargeMapper.updateById(wrkCharge) > 0) {
                        //下发任务
@@ -2824,7 +2831,7 @@
                wrkCharge.setWrkSts(54L);//提升机搬运中
                //所需命令组合完毕，更新数据库，提交到线程去工作
                LiftAssignCommand assignCommand = new LiftAssignCommand();
                assignCommand.setCommands(commands);
//                assignCommand.setCommands(commands);
                assignCommand.setLiftNo(liftProtocol.getLiftNo());
                assignCommand.setTaskNo(liftProtocol.getTaskNo());
                if (wrkChargeMapper.updateById(wrkCharge) > 0) {
@@ -2879,7 +2886,7 @@
                commands.add(command);

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

                wrkCharge.setWrkSts(56L);//充电中状态
                if (wrkChargeMapper.updateById(wrkCharge) > 0) {