四向车下命令时计算移动距离

Junjie

2023-03-28 6bad808389ec9d4c7331bb471c1410b50893fc1c

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

@@ -7,7 +7,10 @@
import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONObject;
import com.core.common.DateUtils;
import com.core.common.SpringUtils;
import com.core.exception.CoolException;
import com.zy.asrs.entity.BasShuttleOpt;
import com.zy.asrs.service.BasShuttleOptService;
import com.zy.common.model.NavigateNode;
import com.zy.common.utils.CommonUtils;
import com.zy.common.utils.NavigatePositionConvert;
@@ -17,9 +20,7 @@
import com.zy.core.ThreadHandler;
import com.zy.core.cache.MessageQueue;
import com.zy.core.cache.OutputQueue;
import com.zy.core.enums.ShuttleRunDirection;
import com.zy.core.enums.ShuttleStatusType;
import com.zy.core.enums.SlaveType;
import com.zy.core.enums.*;
import com.zy.core.model.ShuttleSlave;
import com.zy.core.model.Task;
import com.zy.core.model.command.ShuttleAssignCommand;
@@ -66,10 +67,10 @@
                    case 1:
                        readStatus();
                        break;
//                    // 写入数据
//                    case 2:
//                        write((ShuttleCommand) task.getData());
//                        break;
                    // 写入数据
                    case 2:
                        write((ShuttleCommand) task.getData());
                        break;
                    //下发任务
                    case 3:
                        assignWork((ShuttleAssignCommand) task.getData());
@@ -117,6 +118,7 @@
                if (null == shuttleProtocol) {
                    shuttleProtocol = new ShuttleProtocol();
                    shuttleProtocol.setShuttleNo(slave.getId().shortValue());
                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
                }

                //----------读取四向穿梭车状态-----------
@@ -170,7 +172,7 @@
                //四向穿梭车空闲、有任务且标记为true，需要执行任务的下一条指令
                if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE && shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getPakMk()) {
                    //执行下一步指令
                    executeWork(shuttleProtocol.getTaskNo());
                    executeWork(shuttleProtocol.getAssignCommand());
                }

            }else {
@@ -178,6 +180,7 @@
                throw new CoolException(MessageFormat.format( "四向穿梭车plc状态信息失败 ===>> [id:{0}] [ip:{1}] [port:{2}]", slave.getId(), slave.getIp(), slave.getPort()));
            }
        } catch (Exception e) {
            e.printStackTrace();
            OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】四向穿梭车plc状态信息失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort()));
            initShuttle();
        }
@@ -189,46 +192,77 @@
            return false;
        }

        command.setShuttleNo(slave.getId());
        command.setShuttleNo(slave.getId().shortValue());
        // 开始任务
        short[] array = new short[17];
        //控制指令字
        array[0] = command.getCommandWord();
        //启始二维编号
        array[1] = command.getStartCodeNum();
        //中间二维编号
        array[2] = command.getMiddleCodeNum();
        //目标二维编号
        array[3] = command.getDistCodeNum();
        if (command.getStartCodeNum() != null) {
            //启始二维编号
            array[1] = command.getStartCodeNum();
        }

        //起点到目标点的距离长度，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
        short[] startToDistDistances = CommonUtils.intToShorts(command.getStartToDistDistance());
        array[4] = startToDistDistances[0];
        array[5] = startToDistDistances[1];
        if (command.getMiddleCodeNum() != null) {
            //中间二维编号
            array[2] = command.getMiddleCodeNum();
        }

        //中间点到目标点的距离长度，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
        short[] middleToDistDistances = CommonUtils.intToShorts(command.getMiddleToDistDistance());
        array[6] = middleToDistDistances[0];
        array[7] = middleToDistDistances[1];
        if (command.getDistCodeNum() != null) {
            //目标二维编号
            array[3] = command.getDistCodeNum();
        }

        //小车运行方向
        array[8] = command.getRunDirection();
        //托盘顶升
        array[9] = command.getPalletLift();
        if (command.getStartToDistDistance() != null) {
            //起点到目标点的距离长度，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
            short[] startToDistDistances = CommonUtils.intToShorts(command.getStartToDistDistance());
            array[4] = startToDistDistances[0];
            array[5] = startToDistDistances[1];
        }

        //小车强制移动距离，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
        short[] forceMoveDistances = CommonUtils.intToShorts(command.getForceMoveDistance());
        array[10] = forceMoveDistances[0];
        array[11] = forceMoveDistances[1];
        if (command.getMiddleToDistDistance() != null) {
            //中间点到目标点的距离长度，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
            short[] middleToDistDistances = CommonUtils.intToShorts(command.getMiddleToDistDistance());
            array[6] = middleToDistDistances[0];
            array[7] = middleToDistDistances[1];
        }

        //充电开关
        array[12] = command.getChargeSwitch();
        //小车IO控制
        array[13] = command.getIOControl();
        //小车运行速度
        array[14] = command.getRunSpeed();
        //小车雷达备用
        array[15] = command.getRadarTmp();
        if (command.getRunDirection() != null) {
            //小车运行方向
            array[8] = command.getRunDirection();
        }

        if (command.getPalletLift() != null) {
            //托盘顶升
            array[9] = command.getPalletLift();
        }

        if (command.getForceMoveDistance() != null) {
            //小车强制移动距离，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
            short[] forceMoveDistances = CommonUtils.intToShorts(command.getForceMoveDistance());
            array[10] = forceMoveDistances[0];
            array[11] = forceMoveDistances[1];
        }

        if (command.getChargeSwitch() != null) {
            //充电开关
            array[12] = command.getChargeSwitch();
        }

        if (command.getIOControl() != null) {
            //小车IO控制
            array[13] = command.getIOControl();
        }

        if (command.getRunSpeed() != null) {
            //小车运行速度
            array[14] = command.getRunSpeed();
        }

        if (command.getRadarTmp() != null) {
            //小车雷达备用
            array[15] = command.getRadarTmp();
        }

        //指令结束位
        array[16] = command.getCommandEnd();

@@ -254,65 +288,114 @@
    }

    //分配任务
    private void assignWork(ShuttleAssignCommand assignCommand) {
        //计算路径
        List<NavigateNode> calc = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getDistLocNo(), "in");
        //获取分段路径
        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
    private void assignWork(ShuttleAssignCommand assignCommand) throws Exception {
        //将此map存入redis中
        HashMap<String, Object> map = new HashMap<>();
        //命令执行步序
        map.put("commandStep", 0);
        //路径数据
        map.put("path", data);

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

        //工作号
        map.put("wrk_no", assignCommand.getTaskNo());
        //命令执行步序
        map.put("commandStep", 0);
        //命令
        map.put("assignCommand", assignCommand);
        shuttleProtocol.setTaskNo(assignCommand.getTaskNo());
        shuttleProtocol.setAssignCommand(assignCommand);
        //任务数据保存到redis
        redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map));

        //执行下发任务
        executeWork(assignCommand.getTaskNo());
        executeWork(assignCommand);
    }

    //执行下发的指令
    private void executeWork(Short taskNo) {
    private void executeWork(ShuttleAssignCommand assignCommand) {
        //读取redis数据
        Object o = redisUtil.get("wrk_no_" + taskNo);
        Object o = redisUtil.get("wrk_no_" + assignCommand.getTaskNo());
        HashMap map = JSON.parseObject(o.toString(), HashMap.class);
        //当前步序
        int commandStep = Integer.parseInt(map.get("commandStep").toString());
        //当前路径数据
        Object data = map.get("path");
        ArrayList pathList = JSON.parseObject(data.toString(), ArrayList.class);
        //取第一条路径
        Object o1 = pathList.get(commandStep);
        ArrayList path = JSON.parseObject(o1.toString(), ArrayList.class);

        int size = path.size();
        //开始路径
        JSONObject startPath = JSON.parseObject(path.get(0).toString());
        System.out.println(startPath);
        //目标路径
        JSONObject endPath = JSON.parseObject(path.get(size - 1).toString());
        System.out.println(endPath);

        //path路径数目
        int size = 0;
        //下发命令
        ShuttleCommand command = new ShuttleCommand();
        command.setCommandWord((short) 1);
        command.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getIntValue("x"), startPath.getIntValue("y")));
        command.setMiddleCodeNum((short) 0);
        command.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getIntValue("x"), endPath.getIntValue("y")));
        command.setStartToDistDistance(1000);
        command.setMiddleToDistDistance(1000);
        command.setRunDirection(ShuttleRunDirection.get(startPath.get("direction").toString()).id);
        command.setPalletLift((short) 1);
        command.setForceMoveDistance(1000);
        command.setChargeSwitch((short) 2);
        command.setIOControl((short) 0);
        command.setRunSpeed((short) 1000);
        command.setRadarTmp((short) 0);
        command.setCommandEnd((short) 1);
        switch (assignCommand.getTaskMode()) {
            case 1://入库
            case 2://出库
                //当前路径数据
                Object data = map.get("path");
                ArrayList pathList = JSON.parseObject(data.toString(), ArrayList.class);
                //取第一条路径
                Object o1 = pathList.get(commandStep);
                ArrayList objectPath = JSON.parseObject(o1.toString(), ArrayList.class);
                ArrayList<NavigateNode> path = new ArrayList<>();
                for (Object o2 : objectPath) {
                    NavigateNode navigateNode = JSON.parseObject(o2.toString(), NavigateNode.class);
                    path.add(navigateNode);
                }
                Integer allDistance = NavigateUtils.getCurrentPathAllDistance(path);//计算当前路径行走总距离

                size = Integer.parseInt(map.get("pathSize").toString());
                //开始路径
                NavigateNode startPath = path.get(0);
                //目标路径
                NavigateNode endPath = path.get(path.size() - 1);

                command.setCommandWord((short) 1);
                command.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY()));
                command.setMiddleCodeNum((short) 0);
                command.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY()));
                command.setStartToDistDistance(allDistance);
                command.setMiddleToDistDistance(1000);
                command.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
                command.setPalletLift((short) 1);
                command.setForceMoveDistance(1000);
                command.setChargeSwitch((short) 2);
                command.setIOControl((short) 0);
                command.setRunSpeed((short) 1000);
                command.setRadarTmp((short) 0);

                break;
            case 3://托盘顶升
            case 4://托盘下降
                command.setCommandWord((short) 2);
                command.setPalletLift(assignCommand.getTaskMode() == 3 ? (short)1 : (short)2);
                command.setCommandEnd((short) 1);
                break;
            case 5://强制左移
            case 6://强制右移
            case 7://强制上移
            case 8://强制下移
                command.setCommandWord((short) 3);
                command.setForceMoveDistance(1000);
                command.setRunDirection((short) (assignCommand.getTaskMode() - 4));
                command.setCommandEnd((short) 1);
                break;
            case 9://状态复位
                command.setCommandWord((short) 0);
                //设置四向穿梭车为空闲状态
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
                //任务号清零
                shuttleProtocol.setTaskNo((short) 0);
                break;
            default:
        }

        command.setCommandEnd((short) 1);
        //下发命令
        if (!write(command)) {
            News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
@@ -323,7 +406,7 @@
            shuttleProtocol.setPakMk(false);

            //判断数据是否执行完成
            if (commandStep < size) {
            if (commandStep < size - 1) {
                //更新redis数据
                //步序增加
                commandStep++;
@@ -333,12 +416,33 @@
            }else {
                //已执行完成
                //保存数据到数据库做流水
                BasShuttleOptService shuttleOptService = SpringUtils.getBean(BasShuttleOptService.class);
                if (shuttleOptService != null) {
                    BasShuttleOpt opt = new BasShuttleOpt(
                            assignCommand.getTaskNo().intValue(),
                            assignCommand.getShuttleNo().intValue(),
                            new Date(),
                            ShuttleTaskModeType.get(assignCommand.getTaskMode()).desc,
                            assignCommand.getSourceLocNo(),
                            assignCommand.getDistLocNo(),
                            null,
                            null,
                            null,
                            JSON.toJSONString(command)
                    );
                    shuttleOptService.insert(opt);
                }
                //删除redis
                redisUtil.del("wrk_no_" + map.get("wrk_no").toString());

                //。。。
                //1、命令下方需要判断小车空闲状态
                //2、
                //对主线程抛出等待确认状态waiting
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WAITING);
                //设置四向穿梭车为空闲状态
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
                //任务号清零
                shuttleProtocol.setTaskNo((short) 0);

                News.info("四向穿梭车任务执行完成等待确认中，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
            }

        }