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.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; 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.enums.*; import com.zy.core.model.ShuttleSlave; import com.zy.core.model.Task; import com.zy.core.model.command.ShuttleAssignCommand; import com.zy.core.model.command.ShuttleCommand; 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 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 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())); //小车处于忙碌状态，将标记置为true if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.BUSY) { shuttleProtocol.setPakMk(true); } //四向穿梭车空闲、有任务且标记为true，需要执行任务的下一条指令 if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE && shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getPakMk()) { //执行下一步指令 executeWork(shuttleProtocol.getAssignCommand()); } }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) throws Exception { //将此map存入redis中 HashMap map = new HashMap<>(); //计算路径 List calc = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getDistLocNo(), assignCommand.getTaskMode().intValue()); if (calc != null) { //获取分段路径 ArrayList> 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); } //执行下发的指令 private void executeWork(ShuttleAssignCommand assignCommand) { //读取redis数据 Object o = redisUtil.get("wrk_no_" + assignCommand.getTaskNo()); HashMap map = JSON.parseObject(o.toString(), HashMap.class); //当前步序 int commandStep = Integer.parseInt(map.get("commandStep").toString()); //path路径数目 int size = 0; //下发命令 ShuttleCommand command = new ShuttleCommand(); 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 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)); } else { News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command)); //将标记置为false(防止重发) shuttleProtocol.setPakMk(false); //判断数据是否执行完成 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.getDistLocNo(), null, null, null, JSON.toJSONString(command) ); shuttleOptService.insert(opt); } //删除redis redisUtil.del("wrk_no_" + map.get("wrk_no").toString()); //对主线程抛出等待确认状态waiting shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WAITING); //设置四向穿梭车为空闲状态 shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE); //任务号清零 shuttleProtocol.setTaskNo((short) 0); News.info("四向穿梭车任务执行完成等待确认中，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(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); } }