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.BasDevp; import com.zy.asrs.entity.BasShuttle; import com.zy.asrs.entity.BasShuttleOpt; import com.zy.asrs.entity.LocMast; import com.zy.asrs.service.BasDevpService; import com.zy.asrs.service.BasShuttleOptService; import com.zy.asrs.service.BasShuttleService; import com.zy.asrs.service.LocMastService; import com.zy.common.model.NavigateNode; import com.zy.common.utils.CommonUtils; import com.zy.common.utils.NavigatePositionConvert; import com.zy.common.utils.NavigateUtils; import com.zy.common.utils.RedisUtil; 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.LiftAssignCommand; import com.zy.core.model.command.LiftCommand; import com.zy.core.model.command.ShuttleAssignCommand; import com.zy.core.model.command.ShuttleCommand; import com.zy.core.model.protocol.LiftProtocol; import com.zy.core.model.protocol.ShuttleProtocol; import lombok.Data; import lombok.extern.slf4j.Slf4j; import java.text.MessageFormat; import java.util.ArrayList; import java.util.Date; import java.util.HashMap; 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) { //出现错误 reset(shuttleProtocol.getAssignCommand()); } if(shuttleProtocol.getProtocolStatusType() == ShuttleProtocolStatusType.FIXING && shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE && shuttleProtocol.getAssignCommand() != null){ //处于故障修复状态 //执行下一步指令 executeWork(shuttleProtocol.getAssignCommand()); } //四向穿梭车空闲、有任务、标记为true、存在任务指令,需要执行任务的下一条指令 if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE && shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getPakMk() && shuttleProtocol.getAssignCommand() != null) { //执行下一步指令 executeWork(shuttleProtocol.getAssignCommand()); } //检测是否有提升机锁定标记,有则检测提升机是否到位,是否能走下一步命令 if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE && shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getAssignCommand() != null) { Object o = redisUtil.get("wrk_no_" + shuttleProtocol.getAssignCommand().getTaskNo()); if (o != null) { HashMap map = JSON.parseObject(o.toString(), HashMap.class); if (map.containsKey("liftSecurityMk") && Boolean.parseBoolean(map.get("liftSecurityMk").toString())) { //执行下一步指令 executeWork(shuttleProtocol.getAssignCommand()); } } } //将四向穿梭车状态保存至数据库 BasShuttleService shuttleService = SpringUtils.getBean(BasShuttleService.class); BasShuttle basShuttle = shuttleService.selectById(shuttleProtocol.getShuttleNo()); 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; } 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]; } 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) { //将此map存入redis中 HashMap map = new HashMap<>(); if (!assignCommand.getAuto()) { List commands = new ArrayList<>(); ShuttleCommand command = new ShuttleCommand(); LocMastService locMastService = SpringUtils.getBean(LocMastService.class); 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) { //获取分段路径 ArrayList> data = NavigateUtils.getSectionPath(firstMastResult); //将每一段路径分成command指令 for (ArrayList nodes : data) { //开始路径 NavigateNode startPath = nodes.get(0); //目标路径 NavigateNode endPath = nodes.get(nodes.size() - 1); Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离 ShuttleCommand command1 = new ShuttleCommand(); command1.setCommandWord((short) 1); command1.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), 1)); command1.setMiddleCodeNum((short) 1); command1.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), 1)); command1.setStartToDistDistance(allDistance); command1.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id); command1.setForceMoveDistance(0); command1.setIOControl((short) 0); command1.setRunSpeed((short) 1000); command1.setCommandEnd((short) 1); commands.add(command1); } //托盘顶升 ShuttleCommand command2 = new ShuttleCommand(); command2.setCommandWord((short) 2); command2.setStartCodeNum((short) 0); command2.setMiddleCodeNum((short) 0); command2.setDistCodeNum((short) 0); command2.setStartToDistDistance(0); command2.setMiddleToDistDistance(0); command2.setRunDirection((short) 0); command2.setForceMoveDistance(0); command2.setPalletLift((short) 1); command2.setRunSpeed((short) 0); command2.setCommandEnd((short) 1); commands.add(command2); }else { //没有计算到路径,可能存在小车位置就是起点位置 if (currentLocMast.getLocNo().equals(assignCommand.getSourceLocNo())) { //小车位置就是起点位置,无需移动,直接顶升 //托盘顶升 ShuttleCommand command2 = new ShuttleCommand(); command2.setCommandWord((short) 2); command2.setStartCodeNum((short) 0); command2.setMiddleCodeNum((short) 0); command2.setDistCodeNum((short) 0); command2.setStartToDistDistance(0); command2.setMiddleToDistDistance(0); command2.setRunDirection((short) 0); command2.setForceMoveDistance(0); command2.setPalletLift((short) 1); command2.setRunSpeed((short) 0); command2.setCommandEnd((short) 1); commands.add(command2); } } //计算中点到终点路径 List secMastResult = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id); if (secMastResult != null) { //获取分段路径 ArrayList> data = NavigateUtils.getSectionPath(secMastResult); //将每一段路径分成command指令 for (ArrayList nodes : data) { //开始路径 NavigateNode startPath = nodes.get(0); //目标路径 NavigateNode endPath = nodes.get(nodes.size() - 1); Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离 ShuttleCommand command1 = new ShuttleCommand(); command1.setCommandWord((short) 1); command1.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), 1)); command1.setMiddleCodeNum((short) 1); command1.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), 1)); command1.setStartToDistDistance(allDistance); command1.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id); command1.setForceMoveDistance(0); command1.setIOControl((short) 0); command1.setRunSpeed((short) 1000); command1.setCommandEnd((short) 1); commands.add(command1); } //托盘下降 ShuttleCommand command2 = new ShuttleCommand(); command2.setCommandWord((short) 2); command2.setStartCodeNum((short) 0); command2.setMiddleCodeNum((short) 0); command2.setDistCodeNum((short) 0); command2.setStartToDistDistance(0); command2.setMiddleToDistDistance(0); command2.setRunDirection((short) 0); command2.setForceMoveDistance(0); command2.setPalletLift((short) 2); command2.setRunSpeed((short) 0); command2.setCommandEnd((short) 1); commands.add(command2); } break; case 3://托盘顶升 case 4://托盘下降 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(assignCommand.getTaskMode() == 3 ? (short)1 : (short)2); command.setRunSpeed((short) 0); command.setCommandEnd((short) 1); commands.add(command); break; case 5://强制左移 case 6://强制右移 case 7://强制上移 case 8://强制下移 command.setCommandWord((short) 3); command.setStartCodeNum((short) 0); command.setMiddleCodeNum((short) 0); command.setDistCodeNum((short) 0); command.setStartToDistDistance(600); command.setRunDirection((short) (assignCommand.getTaskMode() - 4)); command.setForceMoveDistance(600); command.setIOControl((short) 0); command.setCommandEnd((short) 1); command.setRunSpeed((short) 1000); commands.add(command); break; case 9://状态复位 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); commands.add(command); break; case 10://向正方向(左)寻库位 command.setCommandWord((short) 4); command.setRunDirection((short) 1); command.setStartToDistDistance(3000); command.setRunSpeed((short) 1000); command.setCommandEnd((short) 1); commands.add(command); break; case 11://向负方向(右)寻库位 command.setCommandWord((short) 4); command.setRunDirection((short) 2); command.setStartToDistDistance(3000); command.setRunSpeed((short) 1000); command.setCommandEnd((short) 1); commands.add(command); break; case 12://向正方向(前)寻库位 command.setCommandWord((short) 4); command.setRunDirection((short) 4); command.setStartToDistDistance(3000); command.setRunSpeed((short) 1000); command.setCommandEnd((short) 1); commands.add(command); break; case 13://向负方向(后)寻库位 command.setCommandWord((short) 4); command.setRunDirection((short) 3); command.setStartToDistDistance(3000); command.setRunSpeed((short) 1000); command.setCommandEnd((short) 1); commands.add(command); break; case 14://移动到目标库位 LocMast locMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString()); List result = NavigateUtils.calc(locMast.getLocNo(), assignCommand.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);//计算当前路径行走总距离 LocMast distLocMast = locMastService.queryByLoc(assignCommand.getLocNo()); String qrCodeValue = distLocMast.getQrCodeValue(); command.setCommandWord((short) 1); command.setStartCodeNum(shuttleProtocol.getCurrentCode()); command.setMiddleCodeNum((short) 1); command.setDistCodeNum((short) Integer.parseInt(qrCodeValue)); command.setStartToDistDistance(allDistance); command.setRunSpeed((short) 1000); command.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id); command.setForceMoveDistance(0); command.setIOControl((short) 0); command.setCommandEnd((short) 1); commands.add(command); } } break; case 15://充电开关 command.setCommandWord((short) 8); command.setIOControl((short) 4); command.setCommandEnd((short) 1); commands.add(command); break; default: } assignCommand.setCommands(commands); } //四向穿梭车号 map.put("shuttle_no", assignCommand.getShuttleNo()); //工作号 map.put("wrk_no", assignCommand.getTaskNo()); //命令执行步序 map.put("commandStep", 0); //命令 map.put("assignCommand", assignCommand); //发生错误时尝试执行的指令,优先级最高 map.put("errorCommands", new ArrayList()); shuttleProtocol.setTaskNo(assignCommand.getTaskNo()); shuttleProtocol.setAssignCommand(assignCommand); shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING); //任务数据保存到redis redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map)); //执行下发任务 executeWork(assignCommand); } //执行下发的指令 private boolean executeWork(ShuttleAssignCommand assignCommand) { //读取redis数据 if (assignCommand == null) { return false; } Object o = redisUtil.get("wrk_no_" + assignCommand.getTaskNo()); if (o == null) { return false; } HashMap map = JSON.parseObject(o.toString(), HashMap.class); if (!checkLiftStation(assignCommand)) {//检测是否有提升机站点,有则调度提升机 return false; } //将标记置为false(防止重发) shuttleProtocol.setPakMk(false); List errorCommands = JSON.parseArray(map.get("errorCommands").toString(),ShuttleCommand.class); 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);//移除该命令 map.put("errorCommands", new ArrayList()); shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING); //当前步序 int commandStep = Integer.parseInt(map.get("commandStep").toString()); //步序回退 commandStep--; map.put("commandStep", commandStep); //任务数据保存到redis redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map)); 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 = Integer.parseInt(map.get("commandStep").toString()); //步序回退 commandStep--; map.put("commandStep", commandStep); } if (!write(errorCommand)) { News.error("四向穿梭车命令下发失败,穿梭车号={},任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand)); return false; } else { News.info("四向穿梭车命令下发成功,穿梭车号={},任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand)); errorCommands.remove(0); map.put("errorCommands", errorCommands); //任务数据保存到redis redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map)); return true; } } List commands = assignCommand.getCommands(); //当前步序 int commandStep = Integer.parseInt(map.get("commandStep").toString()); //path路径数目 int size = commands.size(); //取出命令 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; } } //下发命令 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++; map.put("commandStep", commandStep); //任务数据保存到redis redisUtil.set("wrk_no_" + map.get("wrk_no").toString(), JSON.toJSONString(map)); }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 (map.containsKey("liftSecurityMk") && Boolean.parseBoolean(map.get("liftSecurityMk").toString())) { //曾锁定过提升机,需要进行解锁 LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1); LiftProtocol liftProtocol = liftThread.getLiftProtocol(); if (liftProtocol != null) { liftProtocol.setSecurityMk(false); } } //删除redis redisUtil.del("wrk_no_" + map.get("wrk_no").toString()); if (!assignCommand.getAuto()) { //手动模式不抛出等待状态,直接复位 if (assignCommand.getTaskMode() == 9) { //设置四向穿梭车为空闲状态 shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE); //任务号清零 shuttleProtocol.setTaskNo((short) 0); //标记复位 shuttleProtocol.setPakMk(true); //任务指令清零 shuttleProtocol.setAssignCommand(null); } 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(ShuttleAssignCommand assignCommand) { //读取redis数据 if (assignCommand == null) { return false; } Object o = redisUtil.get("wrk_no_" + assignCommand.getTaskNo()); if (o == null) { return false; } HashMap map = JSON.parseObject(o.toString(), HashMap.class); //当前步序 int commandStep = Integer.parseInt(map.get("commandStep").toString()); //检测是否存在提升机口的指令 List commands = assignCommand.getCommands(); 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; ShuttleCommand command = null; for (int i = 1; i < commands.size(); i++) { command = commands.get(i); if (qrCodeValues.contains(command.getDistCodeNum())) { //存在 step = i + 1; break; } } if (step == 0) { //无需后续检测,直接放行 return true; } //判断下一步是否为提升机口 if (commandStep + 1 != step) { //下一步不是提升机口,跳过后续流程 return true; } //拿到提升机线程 LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1); if (liftThread == null) { return false; } LiftProtocol liftProtocol = liftThread.getLiftProtocol(); if (liftProtocol == null) { return false; } //获取四向穿梭车当前楼层 String shuttleLocNo = shuttleProtocol.getCurrentLocNo();//二维码对应库位号 Integer shuttleLocNoLev = Integer.parseInt(shuttleLocNo.substring(shuttleLocNo.length() - 2, shuttleLocNo.length()));//库位号对应层高 //程序走到这,表示提升机可能一直就在当前层,可能经过了移动到达了该层 if (liftProtocol.getProtocolStatusType() == LiftProtocolStatusType.WAITING) { //提升机等待确认 //设置提升机为空闲状态 liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE); //任务号清零 liftProtocol.setTaskNo((short) 0); //标记复位 liftProtocol.setPakMk(true); //任务指令清零 liftProtocol.setAssignCommand(null); //提升机解锁 liftProtocol.setLiftLock(false); } //判断提升机是否在目标楼层 if (liftProtocol.getLev().intValue() == shuttleLocNoLev) { //同一层,直接放行 return true; } //提升机和穿梭车处于不同楼层,需要进行调度 if (!liftProtocol.isIdle()) { //提升机不是空闲 return false; } //给提升机分配任务 liftProtocol.setLiftLock(true);//锁定提升机 liftProtocol.setTaskNo(shuttleProtocol.getTaskNo());//设置任务号 liftProtocol.setShuttleNo(shuttleProtocol.getShuttleNo());//设置四向穿梭车号 liftProtocol.setProtocolStatus(LiftProtocolStatusType.WORKING);//设置提升机状态为工作中 liftProtocol.setSecurityMk(true);//标记置为true,防止其他任务占用当前提升机 map.put("liftSecurityMk", true);//标记置为true,防止其他任务占用当前提升机 //任务数据保存到redis redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map)); //命令list ArrayList liftCommands = new ArrayList<>(); LiftCommand liftCommand = new LiftCommand(); liftCommand.setLiftNo(liftProtocol.getLiftNo());//提升机号 liftCommand.setTaskNo(liftProtocol.getTaskNo());//任务号 liftCommand.setRun((short) 1);//升降 liftCommand.setDistPosition(shuttleLocNoLev.shortValue());//目标楼层(穿梭车所在楼层) liftCommand.setLiftLock(true);//锁定提升机 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 reset(ShuttleAssignCommand assignCommand) { //读取redis数据 if (assignCommand == null) { return false; } Object o = redisUtil.get("wrk_no_" + assignCommand.getTaskNo()); if (o == null) { return false; } HashMap map = JSON.parseObject(o.toString(), HashMap.class); List commands = assignCommand.getCommands(); //当前步序 int commandStep = Integer.parseInt(map.get("commandStep").toString()); //path路径数目 int size = commands.size(); ArrayList list = new ArrayList<>(); //取出命令 ShuttleCommand command = commands.get(commandStep - 1); //复位命令 ShuttleCommand resetCommand = new ShuttleCommand(); resetCommand.setCommandWord((short) 6); resetCommand.setStartCodeNum((short) 0); resetCommand.setMiddleCodeNum((short) 0); resetCommand.setDistCodeNum((short) 0); resetCommand.setStartToDistDistance(0); resetCommand.setMiddleToDistDistance(0); resetCommand.setRunDirection((short) 0); resetCommand.setPalletLift((short) 0); resetCommand.setPalletLift((short) 0); resetCommand.setForceMoveDistance(0); resetCommand.setChargeSwitch((short) 0); resetCommand.setIOControl((short) 0); resetCommand.setRunSpeed((short) 0); resetCommand.setCommandEnd((short) 1); list.add(resetCommand); //车辆空闲,等待写入找库位命令 //找库位命令 ShuttleCommand searchCommand = new ShuttleCommand(); searchCommand.setCommandWord((short) 4); 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; } searchCommand.setRunDirection(direction);//运行方向 searchCommand.setStartToDistDistance(1200); searchCommand.setRunSpeed((short) 1000); searchCommand.setCommandEnd((short) 1); list.add(searchCommand); //移动车辆,需要在执行完寻找定位点后再执行 ShuttleCommand moveCommand = new ShuttleCommand(); moveCommand.setCommandWord((short) 1); moveCommand.setStartCodeNum(command.getStartCodeNum());//存入目标库位号 list.add(moveCommand); map.put("errorCommands", list); //任务数据保存到redis redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map)); shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.FIXING); return true; } /******************************************************************************************/ /**************************************** 测试专用 *****************************************/ /*****************************************************************************************/ 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); } }