package com.zy.core.thread; import HslCommunication.Core.Transfer.DataFormat; import HslCommunication.Core.Types.OperateResult; import HslCommunication.Core.Types.OperateResultExOne; import HslCommunication.ModBus.ModbusTcpNet; import com.alibaba.fastjson.JSON; import com.core.common.DateUtils; import com.core.common.SpringUtils; import com.core.exception.CoolException; import com.zy.asrs.entity.*; import com.zy.asrs.service.*; import com.zy.asrs.utils.Utils; import com.zy.common.model.NavigateNode; import com.zy.common.utils.*; import com.zy.core.News; import com.zy.core.ThreadHandler; import com.zy.core.cache.MessageQueue; import com.zy.core.cache.OutputQueue; import com.zy.core.cache.SlaveConnection; import com.zy.core.enums.*; import com.zy.core.model.ShuttleSlave; import com.zy.core.model.Task; import com.zy.core.model.command.*; import com.zy.core.model.protocol.LiftProtocol; import com.zy.core.model.protocol.ShuttleProtocol; import com.zy.core.model.protocol.StaProtocol; import lombok.Data; import lombok.extern.slf4j.Slf4j; import java.text.MessageFormat; import java.util.ArrayList; import java.util.Date; import java.util.List; /** * 四向穿梭车线程 */ @Data @Slf4j public class ShuttleThread implements Runnable, ThreadHandler { private ModbusTcpNet modbusTcpNet; private ShuttleSlave slave; private ShuttleProtocol shuttleProtocol; private RedisUtil redisUtil; public ShuttleThread(ShuttleSlave slave,RedisUtil redisUtil) { this.slave = slave; this.redisUtil = redisUtil; } @Override public void run() { this.connect(); while (true) { try { int step = 1; Task task = MessageQueue.poll(SlaveType.Shuttle, slave.getId()); if (task != null) { step = task.getStep(); } switch (step) { // 读数据 case 1: readStatus(); break; // 写入数据 case 2: write((ShuttleCommand) task.getData()); break; //下发任务 case 3: assignWork((ShuttleAssignCommand) task.getData()); break; default: break; } Thread.sleep(500); } catch (Exception e) { e.printStackTrace(); } } } @Override public boolean connect() { boolean result = false; //-------------------------四向穿梭车连接方法------------------------// modbusTcpNet = new ModbusTcpNet(slave.getIp(), slave.getPort(), (byte) 0x01); // 当你需要指定格式的数据解析时，就需要设置下面的这个信息 modbusTcpNet.setDataFormat(DataFormat.ABCD); OperateResult connect = modbusTcpNet.ConnectServer(); if(connect.IsSuccess){ result = true; OutputQueue.CRN.offer(MessageFormat.format( "【{0}】四向穿梭车plc连接成功 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort())); log.info("四向穿梭车plc连接成功 ===>> [id:{}] [ip:{}] [port:{}] ", slave.getId(), slave.getIp(), slave.getPort()); } else { OutputQueue.CRN.offer(MessageFormat.format("【{0}】四向穿梭车plc连接失败！！！ ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort())); log.error("四向穿梭车plc连接失败！！！ ===>> [id:{}] [ip:{}] [port:{}] ", slave.getId(), slave.getIp(), slave.getPort()); } modbusTcpNet.ConnectClose(); //-------------------------四向穿梭车连接方法------------------------// return result; } @Override public void close() { modbusTcpNet.ConnectClose(); } private void readStatus() { try { OperateResultExOne result = modbusTcpNet.Read("200", (short) 17); if (result.IsSuccess) { if (null == shuttleProtocol) { shuttleProtocol = new ShuttleProtocol(); shuttleProtocol.setShuttleNo(slave.getId().shortValue()); shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE); } //----------读取四向穿梭车状态----------- //获取数据 byte[] content = result.Content; //小车忙状态位 shuttleProtocol.setBusyStatus(modbusTcpNet.getByteTransform().TransInt16(content,0)); //当前二维码 shuttleProtocol.setCurrentCode(modbusTcpNet.getByteTransform().TransInt16(content,2)); //电池电量百分比 shuttleProtocol.setBatteryPower(modbusTcpNet.getByteTransform().TransInt16(content,4)); //电池温度 shuttleProtocol.setBatteryTemp(modbusTcpNet.getByteTransform().TransInt16(content,6)); //错误编号 shuttleProtocol.setErrorCode(modbusTcpNet.getByteTransform().TransInt16(content,8)); //Plc输出状态IO boolean[] booleans = modbusTcpNet.getByteTransform().TransBool(content, 10, 2); shuttleProtocol.setPlcOutputLift(booleans[1]); shuttleProtocol.setPlcOutputTransfer(booleans[2]); shuttleProtocol.setPlcOutputBrake(booleans[3]); shuttleProtocol.setPlcOutputCharge(booleans[4]); shuttleProtocol.setPlcOutputStatusIO(modbusTcpNet.getByteTransform().TransInt16(content, 10)); //错误信息码 shuttleProtocol.setStatusErrorCode(modbusTcpNet.getByteTransform().TransInt16(content,12)); //PLC输入状态 shuttleProtocol.setPlcInputStatus(modbusTcpNet.getByteTransform().TransInt16(content,14)); //当前或者之前读到的二维码值 shuttleProtocol.setCurrentOrBeforeCode(modbusTcpNet.getByteTransform().TransInt16(content,16)); //读到的二维码X方向偏移量 shuttleProtocol.setCodeOffsetX(modbusTcpNet.getByteTransform().TransInt16(content,18)); //读到的二维码Y方向偏移量 shuttleProtocol.setCodeOffsetY(modbusTcpNet.getByteTransform().TransInt16(content,20)); //当前的电压值 shuttleProtocol.setCurrentVoltage(modbusTcpNet.getByteTransform().TransInt16(content,22)); //当前的模拟量值 shuttleProtocol.setCurrentAnalogValue(modbusTcpNet.getByteTransform().TransInt16(content,24)); //当前的升降伺服速度 shuttleProtocol.setCurrentLiftServoSpeed(modbusTcpNet.getByteTransform().TransInt16(content,26)); //当前的行走伺服速度 shuttleProtocol.setCurrentMoveServoSpeed(modbusTcpNet.getByteTransform().TransInt16(content,28)); //当前的升降伺服负载率 shuttleProtocol.setCurrentLiftServoLoad(modbusTcpNet.getByteTransform().TransInt16(content,30)); //当前的行走伺服负载率 shuttleProtocol.setCurrentMoveServoLoad(modbusTcpNet.getByteTransform().TransInt16(content,32)); ///读取四向穿梭车状态-end //小车处于忙碌状态，将标记置为true if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.BUSY) { shuttleProtocol.setPakMk(true); } // if (shuttleProtocol.getErrorCode() != 0 && shuttleProtocol.getProtocolStatusType() == ShuttleProtocolStatusType.WORKING) { // //出现错误 // resetAndTryFix(shuttleProtocol.getTaskNo()); // } // // if(shuttleProtocol.getProtocolStatusType() == ShuttleProtocolStatusType.FIXING // && shuttleProtocol.getTaskNo() != 0 // && shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE){ // //处于故障修复状态 // //执行下一步指令 // executeWork(shuttleProtocol.getTaskNo()); // } //四向穿梭车空闲、有任务、标记为true、存在任务指令，需要执行任务的下一条指令 if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE && shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getPakMk()) { //执行下一步指令 executeWork(shuttleProtocol.getTaskNo()); } // //检测是否有提升机锁定标记，有则检测提升机是否到位，是否能走下一步命令 // if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE // && shuttleProtocol.getTaskNo() != 0) { // Object o = redisUtil.get("shuttle_wrk_no_" + shuttleProtocol.getTaskNo()); // if (o != null) { // ShuttleRedisCommand redisCommand = JSON.parseObject(o.toString(), ShuttleRedisCommand.class); // if (redisCommand.getLiftSecurityMk()) { // //执行下一步指令 // executeWork(shuttleProtocol.getTaskNo()); // } // } // } //将四向穿梭车状态保存至数据库 BasShuttleService shuttleService = SpringUtils.getBean(BasShuttleService.class); BasShuttle basShuttle = shuttleService.selectById(slave.getId()); if (basShuttle == null) { basShuttle = new BasShuttle(); //四向穿梭车号 basShuttle.setShuttleNo(slave.getId()); shuttleService.insert(basShuttle); } //小车忙状态位 basShuttle.setBusyStatus(shuttleProtocol.getBusyStatus().intValue()); //当前二维码 basShuttle.setCurrentCode(shuttleProtocol.getCurrentCode().intValue()); //电池电量百分比 basShuttle.setBatteryPower(shuttleProtocol.getBatteryPower().intValue()); //电池温度 basShuttle.setBatteryTemp(shuttleProtocol.getBatteryTemp().intValue()); //错误编号 basShuttle.setErrorCode(shuttleProtocol.getErrorCode().intValue()); //Plc输出状态IO basShuttle.setPlcOutputStatusIo(shuttleProtocol.getPlcOutputStatusIO().intValue()); //错误信息码 basShuttle.setStatusErrorCode(shuttleProtocol.getStatusErrorCode().intValue()); //PLC输入状态 basShuttle.setPlcInputStatus(shuttleProtocol.getPlcInputStatus().intValue()); //当前或者之前读到的二维码值 basShuttle.setCurrentOrBeforeCode(shuttleProtocol.getCurrentOrBeforeCode().intValue()); //读到的二维码X方向偏移量 basShuttle.setCodeOffsetX(shuttleProtocol.getCodeOffsetX().intValue()); //读到的二维码Y方向偏移量 basShuttle.setCodeOffsetY(shuttleProtocol.getCodeOffsetY().intValue()); //当前的电压值 basShuttle.setCurrentVoltage(shuttleProtocol.getCurrentVoltage().intValue()); //当前的模拟量值 basShuttle.setCurrentAnalogValue(shuttleProtocol.getCurrentAnalogValue().intValue()); //当前的升降伺服速度 basShuttle.setCurrentLiftServoSpeed(shuttleProtocol.getCurrentLiftServoSpeed().intValue()); //当前的行走伺服速度 basShuttle.setCurrentMoveServoSpeed(shuttleProtocol.getCurrentMoveServoSpeed().intValue()); //当前的升降伺服负载率 basShuttle.setCurrentLiftServoLoad(shuttleProtocol.getCurrentLiftServoLoad().intValue()); //当前的行走伺服负载率 basShuttle.setCurrentMoveServoLoad(shuttleProtocol.getCurrentMoveServoLoad().intValue()); //当前小车状态（内部自我维护） basShuttle.setShuttleStatus(shuttleProtocol.getProtocolStatus()); //任务号 basShuttle.setWrkNo(shuttleProtocol.getTaskNo().intValue()); //修改时间 basShuttle.setUpdateTime(new Date()); //作业标记 basShuttle.setPakMk(shuttleProtocol.getPakMk()); if (shuttleService.updateById(basShuttle)) { OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】[id:{1}] <<<<< 实时数据更新成功",DateUtils.convert(new Date()), slave.getId())); // log.info(MessageFormat.format("【{0}】[id:{1}] <<<<< 实时数据更新成功",DateUtils.convert(new Date()), slave.getId())); } }else { OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】{1}四向穿梭车plc状态信息失败", DateUtils.convert(new Date()), slave.getId())); 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(); } } private boolean write(ShuttleCommand command){ if (null == command) { News.error("四向穿梭车写入命令为空"); return false; } BasShuttleService shuttleService = SpringUtils.getBean(BasShuttleService.class); if (shuttleService == null) { News.error("系统错误"); return false; } BasShuttle basShuttle = shuttleService.selectById(slave.getId().shortValue()); if (basShuttle == null) { News.error("四向穿梭车不存在"); return false; } command.setShuttleNo(slave.getId().shortValue()); // 开始任务 short[] array = new short[17]; //控制指令字 array[0] = command.getCommandWord(); if (command.getStartCodeNum() != null) { //启始二维编号 array[1] = command.getStartCodeNum(); } if (command.getMiddleCodeNum() != null) { //中间二维编号 array[2] = command.getMiddleCodeNum(); } if (command.getDistCodeNum() != null) { //目标二维编号 array[3] = command.getDistCodeNum(); } if (command.getStartToDistDistance() != null) { //起点到目标点的距离长度，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间 short[] startToDistDistances = CommonUtils.intToShorts(command.getStartToDistDistance()); array[4] = startToDistDistances[0]; array[5] = startToDistDistances[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[8] = basShuttle.getRunSpeed().shortValue();//四向穿梭车运行速度，从系统数据库读出 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(); OperateResult result = modbusTcpNet.Write("0", array);; if (result != null && result.IsSuccess) { News.info("四向穿梭车命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSON(command)); OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSON(command))); return true; } else { OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】写入四向穿梭车plc数据失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort())); News.error("写入四向穿梭车plc数据失败 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); return false; } } /** * 初始化四向穿梭车 */ private void initShuttle() { if (null == shuttleProtocol) { shuttleProtocol = new ShuttleProtocol(); } } //分配任务 private void assignWork(ShuttleAssignCommand assignCommand) { ShuttleRedisCommand redisCommand = new ShuttleRedisCommand(); if (!assignCommand.getAuto()) { List allNode = new ArrayList<>(); List commands = new ArrayList<>(); LocMastService locMastService = SpringUtils.getBean(LocMastService.class); BasShuttleService shuttleService = SpringUtils.getBean(BasShuttleService.class); NavigateMapData navigateMapData; //获取小车移动速度 BasShuttle basShuttle = shuttleService.selectById(slave.getId()); Integer runSpeed = 1000; if (basShuttle != null) { Integer runSpeed1 = basShuttle.getRunSpeed(); if (runSpeed1 != null) { runSpeed = runSpeed1; } } LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1); LiftProtocol liftProtocol = liftThread.getLiftProtocol(); switch (assignCommand.getTaskMode()) { case 1://入库 case 2://出库 //小车移动到提升机口，计算路径 //计算小车起点到中点所需命令 LocMast currentLocMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString()); List firstMastResult = NavigateUtils.calc(currentLocMast.getLocNo(), assignCommand.getSourceLocNo(), ShuttleTaskModeType.PAK_IN.id); if (firstMastResult != null) { allNode.addAll(firstMastResult);//将节点进行保存 //获取分段路径 ArrayList> data = NavigateUtils.getSectionPath(firstMastResult); //将每一段路径分成command指令 for (ArrayList nodes : data) { //开始路径 NavigateNode startPath = nodes.get(0); //中间路径 NavigateNode middlePath = nodes.get(nodes.size() - 2); //目标路径 NavigateNode endPath = nodes.get(nodes.size() - 1); Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离 Integer middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离 //正常移动命令 Short startCode = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());//开始二维码 Short middleCode = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());//目标二维码 Short distCode = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());//目标二维码 commands.add(getMoveCommand(startCode, distCode, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id, middleCode, middleToDistDistance, runSpeed)); } //托盘顶升 commands.add(getPalletCommand((short) 1)); }else { //没有计算到路径，可能存在小车位置就是起点位置 if (currentLocMast.getLocNo().equals(assignCommand.getSourceLocNo())) { //小车位置就是起点位置，无需移动，直接顶升 //托盘顶升 commands.add(getPalletCommand((short) 1)); } } //计算中点到终点路径 List secMastResult = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id); if (secMastResult != null) { allNode.addAll(secMastResult);//将节点进行保存 //获取分段路径 ArrayList> data = NavigateUtils.getSectionPath(secMastResult); //将每一段路径分成command指令 for (ArrayList nodes : data) { //开始路径 NavigateNode startPath = nodes.get(0); //中间路径 NavigateNode middlePath = nodes.get(nodes.size() - 2); //目标路径 NavigateNode endPath = nodes.get(nodes.size() - 1); Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离 Integer middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离 //正常移动命令 Short startCode = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());//开始二维码 Short middleCode = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());//中间二维码 Short distCode = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());//目标二维码 commands.add(getMoveCommand(startCode, distCode, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id, middleCode, middleToDistDistance, runSpeed)); } //托盘下降 commands.add(getPalletCommand((short) 2)); } navigateMapData = new NavigateMapData(Utils.getLev(currentLocMast.getLocNo())); if (firstMastResult != null) { //所使用的路径进行锁定禁用 navigateMapData.writeNavigateNodeToRedisMap(firstMastResult, true);////所使用的路径进行锁定禁用 } if (secMastResult != null) { navigateMapData.writeNavigateNodeToRedisMap(secMastResult, true);////所使用的路径进行锁定禁用 } break; case 3://托盘顶升 case 4://托盘下降 commands.add(getPalletCommand(assignCommand.getTaskMode() == 3 ? (short) 1 : (short) 2)); break; case 5://强制左移 commands.add(getForceMoveCommand((short) 2)); break; case 6://强制右移 commands.add(getForceMoveCommand((short) 1)); break; case 7://强制上移 commands.add(getForceMoveCommand((short) 3)); break; case 8://强制下移 commands.add(getForceMoveCommand((short) 4)); break; case 9://状态复位 ShuttleCommand reset = getResetCommand(); commands.add(reset); break; case 10://正方向(右)寻库位 commands.add(getFindLocCommand((short) 1)); break; case 11://负方向(左)寻库位 commands.add(getFindLocCommand((short) 2)); break; case 12://向正方向(前)寻库位 commands.add(getFindLocCommand((short) 4)); break; case 13://向负方向(后)寻库位 commands.add(getFindLocCommand((short) 3)); break; case 14://移动到目标库位 String startQr = shuttleProtocol.getCurrentCode().toString();//起始位置 //如果穿梭车在提升机内，移动时需要先下发出提升机命令 if (liftProtocol.getBarcode().intValue() == shuttleProtocol.getCurrentCode().intValue()) { //穿梭车出提升机 Short liftLev = liftProtocol.getPositionArrivalFeedback();//提升机位置 String liftSiteLocNo = Utils.levToOutInStaLocNo(liftLev.intValue()); LocMast locMast1 = locMastService.selectById(liftSiteLocNo); ShuttleCommand moveCommand = getMoveCommand(liftProtocol.getBarcode(), Short.parseShort(locMast1.getQrCodeValue()), 1400, ShuttleRunDirection.BOTTOM.id, liftProtocol.getBarcode(), 1400, runSpeed); commands.add(moveCommand); //起始位置修改为提升机口站点位置 startQr = locMast1.getQrCodeValue(); } LocMast locMast = locMastService.queryByQrCode(startQr); List result = NavigateUtils.calc(locMast.getLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id); if (result != null) { //所使用的路径进行锁定禁用 navigateMapData = new NavigateMapData(Utils.getLev(locMast.getLocNo())); navigateMapData.writeNavigateNodeToRedisMap(result, true);////所使用的路径进行锁定禁用 allNode.addAll(result);//将节点进行保存 //获取分段路径 ArrayList> data = NavigateUtils.getSectionPath(result); //将每一段路径分成command指令 for (ArrayList nodes : data) { //开始路径 NavigateNode startPath = nodes.get(0); //中间路径 NavigateNode middlePath = nodes.get(nodes.size() - 2); //目标路径 NavigateNode endPath = nodes.get(nodes.size() - 1); Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离 Integer middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离 Short startCode = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());//开始二维码 Short middleCode = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());//中间二维码 Short distCode = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());//目标二维码 //正常移动命令 commands.add(getMoveCommand(startCode, distCode, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id, middleCode, middleToDistDistance, runSpeed)); } } break; case 15://充电开关 commands.add(getChargeSwitchCommand()); break; case 16://移动到提升机 LocMast locMast1 = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString()); int lev = Utils.getLev(locMast1.getLocNo());//穿梭车当前高度 String liftSiteLocNo = Utils.levToOutInStaLocNo(lev);//当前楼层站点库位号 LocMast liftSitelocMast = locMastService.selectById(liftSiteLocNo); List result1 = NavigateUtils.calc(locMast1.getLocNo(), liftSiteLocNo, ShuttleTaskModeType.PAK_IN.id); Short endStartCode = null; if (result1 != null) { //所使用的路径进行锁定禁用 navigateMapData = new NavigateMapData(Utils.getLev(locMast1.getLocNo())); navigateMapData.writeNavigateNodeToRedisMap(result1, true);////所使用的路径进行锁定禁用 allNode.addAll(result1);//将节点进行保存 //获取分段路径 ArrayList> data = NavigateUtils.getSectionPath(result1); //将每一段路径分成command指令 for (ArrayList nodes : data) { //开始路径 NavigateNode startPath = nodes.get(0); //中间路径 NavigateNode middlePath = nodes.get(nodes.size() - 2); //目标路径 NavigateNode endPath = nodes.get(nodes.size() - 1); Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离 Integer middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离 Short startCode = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());//开始二维码 Short middleCode = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());//中间二维码 Short distCode = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());//目标二维码 endStartCode = distCode; //正常移动命令 commands.add(getMoveCommand(startCode, distCode, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id, middleCode, middleToDistDistance, runSpeed)); } } if (endStartCode == null && shuttleProtocol.getCurrentCode() == Short.parseShort(liftSitelocMast.getQrCodeValue())) { //穿梭车已经在提升机站点口 endStartCode = shuttleProtocol.getCurrentCode(); } //增加移动进提升机命令 ShuttleCommand moveCommand = getMoveCommand(endStartCode, liftProtocol.getBarcode(), 1400, ShuttleRunDirection.TOP.id, endStartCode, 1400, runSpeed); commands.add(moveCommand); break; default: } assignCommand.setCommands(commands); assignCommand.setNodes(allNode);//当前任务所占用的节点list } redisCommand.setShuttleNo(assignCommand.getShuttleNo());//四向穿梭车号 redisCommand.setWrkNo(assignCommand.getTaskNo());//工作号 redisCommand.setCommandStep(0);//命令执行步序 redisCommand.setAssignCommand(assignCommand);//命令 redisCommand.setErrorCommands(new ArrayList());//发生错误时尝试执行的指令，优先级最高 shuttleProtocol.setTaskNo(assignCommand.getTaskNo()); shuttleProtocol.setAssignCommand(assignCommand); shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING); //任务数据保存到redis redisUtil.set("shuttle_wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(redisCommand)); //执行下发任务 executeWork(assignCommand.getTaskNo()); } //执行下发的指令 private boolean executeWork(Short wrkNo) { //读取redis数据 if (wrkNo == null) { return false; } Object o = redisUtil.get("shuttle_wrk_no_" + wrkNo); if (o == null) { return false; } ShuttleRedisCommand redisCommand = JSON.parseObject(o.toString(), ShuttleRedisCommand.class); if (!checkLiftStation(wrkNo)) {//检测是否有提升机站点，有则调度提升机 return false; } //将标记置为false(防止重发) shuttleProtocol.setPakMk(false); List errorCommands = redisCommand.getErrorCommands(); if (errorCommands.size() > 0) { //优先执行该指令 ShuttleCommand errorCommand = errorCommands.get(0);//取出指令 if(errorCommand.getCommandWord() == 1){//正常行走命令，需要先执行完找库位命令后，再执行 LocMastService locMastService = SpringUtils.getBean(LocMastService.class); LocMast locMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString()); LocMast distLocMast = locMastService.queryByQrCode(errorCommand.getStartCodeNum().toString()); if (shuttleProtocol.getCurrentCode().equals(errorCommand.getStartCodeNum())) { //起点和终点属于同一库位，无需再执行移动操作 errorCommands.remove(0);//移除该命令 redisCommand.setErrorCommands(new ArrayList()); shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING); //当前步序 int commandStep = redisCommand.getCommandStep(); //步序回退 commandStep--; redisCommand.setCommandStep(commandStep); //任务数据保存到redis redisUtil.set("shuttle_wrk_no_" + wrkNo, JSON.toJSONString(redisCommand)); shuttleProtocol.setPakMk(true); return true; }else { List result = NavigateUtils.calc(locMast.getLocNo(), distLocMast.getLocNo(), ShuttleTaskModeType.PAK_IN.id); if (result != null) { //获取分段路径 ArrayList> data = NavigateUtils.getSectionPath(result); //将每一段路径分成command指令 for (ArrayList nodes : data) { //开始路径 NavigateNode startPath = nodes.get(0); //目标路径 NavigateNode endPath = nodes.get(nodes.size() - 1); Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离 String qrCodeValue = distLocMast.getQrCodeValue(); errorCommand.setCommandWord((short) 1); errorCommand.setStartCodeNum(shuttleProtocol.getCurrentCode()); errorCommand.setMiddleCodeNum((short) 1); errorCommand.setDistCodeNum((short) Integer.parseInt(qrCodeValue)); errorCommand.setStartToDistDistance(allDistance); errorCommand.setRunSpeed((short) 1000); errorCommand.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id); errorCommand.setForceMoveDistance(0); errorCommand.setIOControl((short) 0); errorCommand.setCommandEnd((short) 1); break; } } } shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING); //当前步序 int commandStep = redisCommand.getCommandStep(); //步序回退 commandStep--; redisCommand.setCommandStep(commandStep); } if (!write(errorCommand)) { News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand)); return false; } else { News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand)); errorCommands.remove(0); redisCommand.setErrorCommands(errorCommands); //任务数据保存到redis redisUtil.set("shuttle_wrk_no_" + wrkNo, JSON.toJSONString(redisCommand)); return true; } } List commands = redisCommand.getAssignCommand().getCommands(); //当前步序 int commandStep = redisCommand.getCommandStep(); //path路径数目 int size = commands.size(); ShuttleAssignCommand assignCommand = redisCommand.getAssignCommand(); //取出命令 ShuttleCommand command = commands.get(commandStep); if (assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_IN.id.shortValue() || assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_OUT.id.shortValue() ) { //小车失去坐标，禁止下发命令 if (shuttleProtocol.getCurrentCode() == 0) { return false; } } LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1); LiftProtocol liftProtocol = liftThread.getLiftProtocol(); //下发命令 if (!write(command)) { News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command)); return false; } else { News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command)); //判断数据是否执行完成 if (commandStep < size - 1) { //更新redis数据 //步序增加 commandStep++; redisCommand.setCommandStep(commandStep); //任务数据保存到redis redisUtil.set("shuttle_wrk_no_" + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand)); }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.getLocNo(), null, null, null, JSON.toJSONString(assignCommand) ); shuttleOptService.insert(opt); } if (redisCommand.getLiftSecurityMk()) { //曾锁定过提升机，需要进行解锁 if (liftProtocol != null) { liftProtocol.setSecurityMk(false); } } String locNo = shuttleProtocol.getLocNo() == null ? shuttleProtocol.getSourceLocNo() : shuttleProtocol.getLocNo(); if (locNo != null) { //解除锁定的库位路径 NavigateMapData navigateMapData = new NavigateMapData(Utils.getLev(locNo)); navigateMapData.writeNavigateNodeToRedisMap(redisCommand.getAssignCommand().getNodes(), false); } //删除redis redisUtil.del("shuttle_wrk_no_" + redisCommand.getWrkNo()); if (!assignCommand.getAuto()) { //手动模式不抛出等待状态，直接复位 //设置四向穿梭车为空闲状态 shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE); //任务号清零 shuttleProtocol.setTaskNo((short) 0); //标记复位 shuttleProtocol.setPakMk(true); News.info("四向穿梭车手动任务执行完成，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command)); }else { if (!assignCommand.getCharge()) { //对主线程抛出等待确认状态waiting shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WAITING); }else { shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING); } News.info("四向穿梭车任务执行完成等待确认中，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command)); } } } return true; } /** * 检测是否有提升机站点，有则调度提升机 */ private boolean checkLiftStation(Short wrkNo) { //读取redis数据 if (wrkNo == null) { return false; } //拿到提升机线程 LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1); if (liftThread == null) { return false; } LiftProtocol liftProtocol = liftThread.getLiftProtocol(); if (liftProtocol == null) { return false; } Object o = redisUtil.get("shuttle_wrk_no_" + wrkNo); if (o == null) { return false; } ShuttleRedisCommand redisCommand = JSON.parseObject(o.toString(), ShuttleRedisCommand.class); //当前步序 int commandStep = redisCommand.getCommandStep(); //检测是否存在提升机口的指令 List commands = redisCommand.getAssignCommand().getCommands(); if (commands.size() > 0) { if (commands.get(commandStep).getCommandWord() != 1) { //不是行走命令，直接放行 return true; } if (commands.get(0).getStartCodeNum() == null) { return false; } //命令起始位置就是提升机二维码，则不进行校验 if (commands.get(0).getStartCodeNum().intValue() == liftProtocol.getBarcode().intValue()) { return true; } } BasDevpService basDevpService = SpringUtils.getBean(BasDevpService.class); ArrayList qrCodeValues = new ArrayList<>(); for (BasDevp basDevp : basDevpService.selectList(null)) { //将所有提升机口二维码存入list qrCodeValues.add(Short.parseShort(basDevp.getQrCodeValue())); } //遍历所有指令，判断是否有到提升机口的指令，并获取到达该提升机口所需步序 int step = 0; Integer siteNo = null;//站点号 ShuttleCommand command = null; for (int i = 0; i < commands.size(); i++) { command = commands.get(i); for (Short qrCodeValue : qrCodeValues) { if (qrCodeValue.intValue() == command.getDistCodeNum().intValue()) { //存在 step = i + 1; BasDevp basDevp = basDevpService.queryByQrCode(qrCodeValue.intValue()); siteNo = basDevp.getDevNo(); break; } } } if (step == 0) { //无需后续检测，直接放行 return true; } //判断下一步是否为提升机口 if (commandStep + 1 != step) { //下一步不是提升机口，跳过后续流程 return true; } //获取四向穿梭车当前楼层 String shuttleLocNo = shuttleProtocol.getCurrentLocNo();//二维码对应库位号 Integer shuttleLocNoLev = shuttleLocNo == null ? 0 : Utils.getLev(shuttleLocNo);//库位号对应层高 //判断提升机是否在目标楼层 if (shuttleLocNoLev >= 2) { shuttleLocNoLev++; } //判断输送线站点是否给出提升机到位信号 if (siteNo != null) { SiemensDevpThread siemensDevpThread = (SiemensDevpThread) SlaveConnection.get(SlaveType.Devp, 1); StaProtocol staProtocol = siemensDevpThread.getStation().get(siteNo); if (staProtocol.isLiftArrival() && liftProtocol.getPositionArrivalFeedback().intValue() == shuttleLocNoLev) { //输送线反馈提升机到位且提升机楼层反馈为同一层，直接放行 return true; } } if (liftProtocol.getRunning()) { //提升机运行中，禁止下发 return false; } if (redisCommand.getLiftSecurityMk()) { //已经执行过提升机命令，禁止下发 return false; } //给提升机分配任务 liftProtocol.setTaskNo(shuttleProtocol.getTaskNo());//设置任务号 liftProtocol.setShuttleNo(shuttleProtocol.getShuttleNo());//设置四向穿梭车号 liftProtocol.setProtocolStatus(LiftProtocolStatusType.WORKING);//设置提升机状态为工作中 liftProtocol.setSecurityMk(true);//标记置为true，防止其他任务占用当前提升机 redisCommand.setLiftSecurityMk(true);//标记置为true，防止其他任务占用当前提升机 //任务数据保存到redis redisUtil.set("shuttle_wrk_no_" + wrkNo, JSON.toJSONString(redisCommand)); //命令list ArrayList liftCommands = new ArrayList<>(); LiftCommand liftCommand = liftThread.getLiftUpDownCommand(liftProtocol.getLiftNo(), liftProtocol.getTaskNo(), shuttleLocNoLev); liftCommands.add(liftCommand);//将命令添加进list LiftAssignCommand liftAssignCommand = new LiftAssignCommand(); liftAssignCommand.setCommands(liftCommands); liftAssignCommand.setLiftNo(liftProtocol.getLiftNo()); liftAssignCommand.setTaskNo(liftProtocol.getTaskNo()); //下发任务 MessageQueue.offer(SlaveType.Lift, liftProtocol.getLiftNo().intValue(), new Task(3, liftAssignCommand)); return false; } /** * 复位并尝试修复错误 */ private boolean resetAndTryFix(Short wrkNo) { //读取redis数据 if (wrkNo == null) { return false; } Object o = redisUtil.get("shuttle_wrk_no_" + wrkNo); if (o == null) { return false; } ShuttleRedisCommand redisCommand = JSON.parseObject(o.toString(), ShuttleRedisCommand.class); List commands = redisCommand.getAssignCommand().getCommands(); //当前步序 int commandStep = redisCommand.getCommandStep(); //path路径数目 int size = commands.size(); ArrayList list = new ArrayList<>(); //取出命令 ShuttleCommand command = commands.get(commandStep - 1); //复位命令 ShuttleCommand resetCommand = getResetCommand(); list.add(resetCommand); //车辆空闲，等待写入找库位命令 //找库位命令 short direction = 1; switch (command.getRunDirection()) {//转换运行方向 case 1: direction = 2; break; case 2: direction = 1; break; case 3: direction = 4; break; case 4: direction = 3; break; default: direction = 1; } //找库位命令 ShuttleCommand searchCommand = getFindLocCommand(direction, 1200, (short) 1000); list.add(searchCommand); //移动车辆，需要在执行完寻找定位点后再执行 ShuttleCommand moveCommand = new ShuttleCommand(); moveCommand.setCommandWord((short) 1); moveCommand.setStartCodeNum(command.getStartCodeNum());//存入目标库位号 list.add(moveCommand); redisCommand.setErrorCommands(list); //任务数据保存到redis redisUtil.set("shuttle_wrk_no_" + wrkNo, JSON.toJSONString(redisCommand)); shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.FIXING); return true; } /** * 获取托盘顶升命令 * @param lift 1顶升，2下降 */ public ShuttleCommand getPalletCommand(Short lift) { ShuttleCommand command = new ShuttleCommand(); command.setCommandWord((short) 2); command.setStartCodeNum((short) 0); command.setMiddleCodeNum((short) 0); command.setDistCodeNum((short) 0); command.setStartToDistDistance(0); command.setMiddleToDistDistance(0); command.setRunDirection((short) 0); command.setForceMoveDistance(0); command.setPalletLift(lift); command.setRunSpeed((short) 0); command.setCommandEnd((short) 1); return command; } /** * 正常移动命令,默认移动速度1000 */ public ShuttleCommand getMoveCommand(Short startCodeNum, Short distCodeNum, Integer startToDistDistance, Short runDirection, Short middleCodeNum, Integer middleToDistDistance) { return getMoveCommand(startCodeNum, distCodeNum, startToDistDistance, runDirection, middleCodeNum, middleToDistDistance, 1000); } /** * 正常移动命令 */ public ShuttleCommand getMoveCommand(Short startCodeNum, Short distCodeNum, Integer startToDistDistance, Short runDirection, Short middleCodeNum, Integer middleToDistDistance, Integer runSpeed) { ShuttleCommand command = new ShuttleCommand(); command.setCommandWord((short) 1); command.setStartCodeNum(startCodeNum); command.setMiddleCodeNum(middleCodeNum); command.setDistCodeNum(distCodeNum); command.setStartToDistDistance(startToDistDistance); command.setMiddleToDistDistance(middleToDistDistance); command.setRunDirection(runDirection); command.setForceMoveDistance(0); command.setIOControl((short) 0); command.setRunSpeed(runSpeed.shortValue()); command.setCommandEnd((short) 1); return command; } /** * 强制移动命令 * @param direction 移动方向 1左移，2右移，3前移，4后移 */ public ShuttleCommand getForceMoveCommand(Short direction) { ShuttleCommand command = new ShuttleCommand(); command.setCommandWord((short) 3); command.setStartCodeNum((short) 0); command.setMiddleCodeNum((short) 0); command.setDistCodeNum((short) 0); command.setStartToDistDistance(600); command.setRunDirection(direction); command.setForceMoveDistance(600); command.setIOControl((short) 0); command.setCommandEnd((short) 1); command.setRunSpeed((short) 1000); return command; } /** * 状态复位命令 */ public ShuttleCommand getResetCommand() { ShuttleCommand command = new ShuttleCommand(); command.setCommandWord((short) 6); command.setStartCodeNum((short) 0); command.setMiddleCodeNum((short) 0); command.setDistCodeNum((short) 0); command.setStartToDistDistance(0); command.setMiddleToDistDistance(0); command.setRunDirection((short) 0); command.setPalletLift((short) 0); command.setPalletLift((short) 0); command.setForceMoveDistance(0); command.setChargeSwitch((short) 0); command.setIOControl((short) 0); command.setRunSpeed((short) 0); command.setCommandEnd((short) 1); return command; } /** * 获取寻库位命令 * @param direction 1:向正方向(左)寻库位,2:向负方向(右)寻库位,3:向负方向(后)寻库位,4:向正方向(前)寻库位 */ public ShuttleCommand getFindLocCommand(Short direction, Integer startToDistance, Short runSpeed) { ShuttleCommand command = new ShuttleCommand(); command.setCommandWord((short) 4); command.setRunDirection(direction); command.setStartToDistDistance(startToDistance); command.setRunSpeed((short) runSpeed); command.setCommandEnd((short) 1); return command; } /** * 获取寻库位命令，默认移动距离3000，运行速度1000 * @param direction 1:向正方向(左)寻库位,2:向负方向(右)寻库位,3:向负方向(后)寻库位,4:向正方向(前)寻库位 */ public ShuttleCommand getFindLocCommand(Short direction) { return getFindLocCommand(direction, 3000, (short) 1000); } /** * 获取充电开关命令 */ public ShuttleCommand getChargeSwitchCommand() { ShuttleCommand command = new ShuttleCommand(); command.setCommandWord((short) 5); command.setChargeSwitch((short) 1); command.setCommandEnd((short) 1); return command; } /** * charge：1=>开始充电，2=>断开充电 */ public ShuttleCommand getChargeSwitchCommand(Short charge) { ShuttleCommand command = new ShuttleCommand(); command.setCommandWord((short) 5);//充电 command.setShuttleNo(shuttleProtocol.getShuttleNo()); command.setChargeSwitch(charge);//开始充电 command.setCommandEnd((short) 1); return command; } /******************************************************************************************/ /**************************************** 测试专用 *****************************************/ /*****************************************************************************************/ public static void main(String[] args) throws InterruptedException { ShuttleSlave slave = new ShuttleSlave(); slave.setId(1); slave.setIp("192.168.4.24"); slave.setPort(502); // ShuttleThread thread = new ShuttleThread(slave); // thread.connect(); // thread.readStatus(); // // ShuttleCommand command = new ShuttleCommand(); // command.setCommandWord((short) 0); // command.setStartCodeNum((short) 12323); // command.setMiddleCodeNum((short) 22323); // command.setDistCodeNum((short) 29999); // command.setStartToDistDistance(109999); // command.setMiddleToDistDistance(5000); // command.setRunDirection((short) 1); // command.setPalletLift((short) 2); // command.setForceMoveDistance(3000); // command.setChargeSwitch((short) 2); // command.setIOControl((short) 0); // command.setRunSpeed((short) 0); // command.setRadarTmp((short) 0); // command.setCommandEnd((short) 1); // thread.write(command); } }