package com.zy.core.thread; import HslCommunication.Core.Types.OperateResult; import HslCommunication.Core.Types.OperateResultExOne; import HslCommunication.Profinet.Siemens.SiemensPLCS; import HslCommunication.Profinet.Siemens.SiemensS7Net; import com.alibaba.fastjson.JSON; import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper; import com.core.common.DateUtils; import com.core.common.SpringUtils; import com.zy.asrs.entity.BasCircularShuttle; import com.zy.asrs.entity.BasRgv; import com.zy.asrs.entity.BasRgvErrLog; import com.zy.asrs.entity.BasRgvOpt; import com.zy.asrs.service.BasCircularShuttleService; import com.zy.asrs.service.BasDevpPositionService; import com.zy.asrs.service.BasRgvErrLogService; import com.zy.asrs.service.BasRgvOptService; import com.zy.asrs.service.BasRgvService; import com.zy.asrs.utils.NumUtils; 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.RgvModeType; import com.zy.core.enums.RgvTaskModeType; import com.zy.core.enums.SlaveType; import com.zy.core.model.RgvSlave; import com.zy.core.model.Task; import com.zy.core.model.command.RgvCommand; import com.zy.core.model.protocol.RgvProtocol; import lombok.Data; import lombok.extern.slf4j.Slf4j; import java.text.MessageFormat; import java.util.*; import java.util.concurrent.ConcurrentHashMap; /** * RGV线程 * Created by vincent on 2022/11/29 */ @Data @Slf4j public class RgvThread implements Runnable, ThreadHandler { private SiemensS7Net siemensNet; private RgvSlave slave; private RgvProtocol rgvProtocol; /** * 工位1复位信号 */ private boolean resetFlag1 = false; /** * 工位2复位信号 */ private boolean resetFlag2 = false; private boolean connectRgv = false; private boolean alarmChangeSign = false; private boolean csSign = true;//测试标记 /** 模拟移动速度：PLC轨道单位/tick（200ms），5000单位≈2.5m/s */ private static final long SIM_CS_MOVE_STEP = 5000; /** 环形轨道总长（PLC轨道单位） */ private static final long PERIMETER = 1737000; /** 避让安全距离 */ private static final long AVOIDANCE_DISTANCE = 50000; /** 模拟任务阶段 */ private enum SimPhase { IDLE, MOVING, WAITING_AT_SOURCE } /** 模拟任务数据 */ @Data static class SimTask { int taskNo; short sourceSta; short destSta; short mode; // 任务模式：1=取货 2=放货 3=取放货 5=漫游 SimPhase phase; long targetPos; // 目标轨道位置 long startPos; // 起始轨道位置 long destPos; // 放货站位置（仅取放货任务使用） long waitStartTime; // 到达取货站后的等待开始时间 } /** 全局模拟任务表：rgvNo → SimTask，线程安全 */ static final Map simTasks = new ConcurrentHashMap<>(); public RgvThread(RgvSlave slave) { this.slave = slave; } @Override @SuppressWarnings("InfiniteLoopStatement") public void run() { if (csSign) { connectRgv = true; // 模拟模式跳过PLC连接 } else { connectRgv = this.connect(); } // 启动线程自动重连 new Thread(this::rgvConnect).start(); // 启动读数据线程 new Thread(this::readStatusRgv).start(); // 启动任务下发线程 new Thread(this::taskIssued).start(); // 模拟模式：启动模拟运行线程 if (csSign) { new Thread(this::simThreadRun).start(); } } /** * 任务下发 */ private void taskIssued() { while (true) { try { if(!connectRgv){ try { Thread.sleep(1000L); } catch (Exception e){ } continue; } int step = 1; Task task = MessageQueue.poll(SlaveType.Rgv, slave.getId()); if (task != null) { step = task.getStep(); } if (csSign) { // ===== 模拟模式：不写PLC，用模拟方法替代 ===== switch (step) { case 1: taskCompleteCS(); break; case 2: RgvCommand command2 = (RgvCommand) task.getData(); rgvOpt(command2); writeCS(command2); rgvProtocol.setTaskNo1(command2.getTaskNo1()); break; case 3: RgvCommand command = (RgvCommand) task.getData(); if (null == command) { command = new RgvCommand(); } command.setRgvNo(slave.getId()); command.setTaskNo1((short) 0); command.setAckFinish1((short) 1); command.setTaskMode1(RgvTaskModeType.NONE); command.setSourceStaNo1((short)0); command.setDestinationStaNo1((short)0); rgvOpt(command); write3CS(command); break; case 4: RgvCommand command4 = (RgvCommand) task.getData(); rgvOpt(command4); write4CS(command4); break; case 5: Long aLong = (Long) task.getData(); rgvOpt(aLong); write5CS(aLong); break; default: break; } } else { // ===== 真实模式：写PLC ===== switch (step) { //漫游任务完成信号 case 1: // readStatus(); taskComplete(); break; //工位1写入数据 case 2: RgvCommand command2 = (RgvCommand) task.getData(); rgvOpt(command2); write(command2); break; // 复位 case 3: RgvCommand command = (RgvCommand) task.getData(); if (null == command) { command = new RgvCommand(); } command.setRgvNo(slave.getId()); // RGV编号 command.setTaskNo1((short) 0); // 工作号 command.setAckFinish1((short) 1); // 任务完成确认位 command.setTaskMode1(RgvTaskModeType.NONE); // 任务模式 command.setSourceStaNo1((short)0); // 源站 command.setDestinationStaNo1((short)0); // 目标站 rgvOpt(command); write3(command); break; //工位1写入取消数据 case 4: RgvCommand command4 = (RgvCommand) task.getData(); rgvOpt(command4); write4(command4); break; // 漫游 case 5: Long aLong = (Long) task.getData(); rgvOpt(aLong); write5(aLong); break; default: break; } } Thread.sleep(50); } catch (Exception e) { log.error("RGV写线程异常"+e.getMessage()); // e.printStackTrace(); } } } private void rgvConnect() { while (true) { try { Thread.sleep(1000); if(!connectRgv){ try { // 根据实时信息更新数据库 BasCircularShuttleService basCircularShuttleService = SpringUtils.getBean(BasCircularShuttleService.class); BasCircularShuttle basCircularShuttle = basCircularShuttleService.getOne(new QueryWrapper().eq("rgv_no", slave.getId())); if (basCircularShuttle.getStatus() != 0){ continue; } } catch (Exception ignore) { } try { connectRgv = this.connect(); Thread.sleep(100); } catch (Exception e){ } } } catch (Exception e) { log.error("rgv连接失败！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); initRgv(); // e.printStackTrace(); } } } private void readStatusRgv() { while (true) { try { if(!connectRgv){ try { Thread.sleep(1000L); } catch (Exception e){ } initRgv(); continue; } Thread.sleep(40); // readStatus(); initRgv(); } catch (Exception e) { log.error("RGV读线程异常"+e.getMessage()); log.error("RGV数据读取线程异常！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); initRgv(); // e.printStackTrace(); } } } /** * 初始化RGV状态 */ private void initRgv() { if (csSign){ initRgvCS(); } else { initRgvT(); } } /** * 初始化RGV状态 */ private void initRgvT() { if (null == rgvProtocol) { rgvProtocol = new RgvProtocol(); } rgvProtocol.setRgvNo(slave.getId()); rgvProtocol.setMode((short) -1); rgvProtocol.setStatus((short)-1); rgvProtocol.setTaskNo1((short)0); rgvProtocol.setStatus1((short)-1); rgvProtocol.setLoaded1((short)0); rgvProtocol.setWalkPos((short)0); rgvProtocol.setRgvPos(0L); rgvProtocol.setTaskNo2((short)0); rgvProtocol.setStatus2((short)-1); rgvProtocol.setLoaded2((short)0); rgvProtocol.setAlarm((short)0); rgvProtocol.setxSpeed((short) 0); rgvProtocol.setxDistance((short) 0); rgvProtocol.setxDuration((short) 0); log.error("连接中断：RGV号："+slave.getId()); try { // 根据实时信息更新数据库 BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class); BasRgv basRgv = new BasRgv(); basRgv.setRgvNo(slave.getId()); basRgv.setRgvSts((int)rgvProtocol.getMode()); basRgv.setLoaded2(rgvProtocol.getLoaded2().intValue()); if (!basRgvService.updateById(rgvProtocol.toSqlModel(basRgv))){ log.error("RGV plc数据库更新失败 ===>> [id:{}] [ip:{}] [port:{}] [rack:{}] [slot:{}]", slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot()); } } catch (Exception ignore) { } } /** * 初始化RGV状态 */ private void initRgvCS() { if (null == rgvProtocol) { rgvProtocol = new RgvProtocol(); rgvProtocol.setRgvNo(slave.getId()); rgvProtocol.setMode((short) 3); rgvProtocol.setStatus((short)0); rgvProtocol.setTaskNo1((short)0); rgvProtocol.setStatus1((short)0); rgvProtocol.setLoaded1((short)0); rgvProtocol.setWalkPos((short)0); if (rgvProtocol.getRgvPos()==null || rgvProtocol.getRgvPos()==0){ rgvProtocol.setRgvPos(1L+rgvProtocol.getRgvNo()*100000); } // rgvProtocol.setRgvPos(rgvProtocol.getRgvPos()+1000); rgvProtocol.setRgvPos(rgvProtocol.getRgvPos()); if (rgvProtocol.getRgvPos()>0){ rgvProtocol.setRgvPos((long)rgvProtocol.getRgvPos()); rgvProtocol.setRgvPosInt(rgvProtocol.getRgvPos().intValue()); } rgvProtocol.setTaskNo2((short)0); rgvProtocol.setStatus2((short)-1); rgvProtocol.setLoaded2((short)0); rgvProtocol.setAlarm((short)0); rgvProtocol.setxSpeed((short) 0); rgvProtocol.setxDistance((short) 0); rgvProtocol.setxDuration((short) 0); } // log.error("连接中断：RGV号："+slave.getId()); try { // 根据实时信息更新数据库 BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class); BasRgv basRgv = new BasRgv(); basRgv.setRgvNo(slave.getId()); basRgv.setRgvSts((int)rgvProtocol.getMode()); basRgv.setLoaded2(rgvProtocol.getLoaded2().intValue()); if (!basRgvService.updateById(rgvProtocol.toSqlModel(basRgv))){ log.error("RGV plc数据库更新失败 ===>> [id:{}] [ip:{}] [port:{}] [rack:{}] [slot:{}]", slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot()); } } catch (Exception ignore) { } } @Override public boolean connect() { boolean result = false; siemensNet = new SiemensS7Net(SiemensPLCS.S1200, slave.getIp()); siemensNet.setRack(slave.getRack().byteValue()); siemensNet.setSlot(slave.getSlot().byteValue()); OperateResult connect = siemensNet.ConnectServer(); if(connect.IsSuccess){ result = true; OutputQueue.RGV.offer(MessageFormat.format( "【{0}】RGV plc连接成功 ===>> [id:{1}] [ip:{2}] [port:{3}] [rack:{4}] [slot:{5}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot())); log.info("RGV plc连接成功 ===>> [id:{}] [ip:{}] [port:{}] [rack:{}] [slot:{}]", slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot()); } else { OutputQueue.RGV.offer(MessageFormat.format("【{0}】RGV plc连接失败！！！ ===>> [id:{1}] [ip:{2}] [port:{3}] [rack:{4}] [slot:{5}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot())); log.error("RGV plc连接失败！！！ ===>> [id:{}] [ip:{}] [port:{}] [rack:{}] [slot:{}]", slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot()); initRgv(); } // siemensNet.ConnectClose(); return result; } /** * 读取状态 */ private void readStatus(){ try { OperateResultExOne result = siemensNet.Read("DB100.0", (short) 20); OperateResultExOne resultV = siemensNet.Read("DB20.16", (short) 2); OperateResultExOne resultE = siemensNet.Read("DB20.26", (short) 2); OperateResultExOne resultError = siemensNet.Read("DB13.0", (short) 13); if (result.IsSuccess && resultV.IsSuccess && resultE.IsSuccess) { if (null == rgvProtocol) { rgvProtocol = new RgvProtocol(); rgvProtocol.setRgvNo(slave.getId()); } rgvProtocol.setRgvNo(slave.getId()); rgvProtocol.setMode(siemensNet.getByteTransform().TransInt16(result.Content, 2)); rgvProtocol.setStartSta(siemensNet.getByteTransform().TransInt16(result.Content, 4)); rgvProtocol.setEndSta(siemensNet.getByteTransform().TransInt16(result.Content, 6)); rgvProtocol.setTaskNo1(siemensNet.getByteTransform().TransInt16(result.Content, 8)); rgvProtocol.setAlarm(siemensNet.getByteTransform().TransInt16(result.Content, 10)); rgvProtocol.setStatus(siemensNet.getByteTransform().TransInt16(result.Content, 12)); rgvProtocol.setxSpeed(siemensNet.getByteTransform().TransInt16(result.Content, 14)); int poi = siemensNet.getByteTransform().TransInt32(result.Content, 16); if (poi>0){ rgvProtocol.setRgvPos((long)poi); rgvProtocol.setRgvPosInt(poi); } log.info(rgvProtocol.getRgvNo()+"号小车读取定位值："+poi); rgvProtocol.setInstantaneousSpeed(Double.valueOf(siemensNet.getByteTransform().TransInt16(resultV.Content, 0))); rgvProtocol.setEndStaM(siemensNet.getByteTransform().TransInt16(resultE.Content, 0)); boolean[] statusAlarmList = siemensNet.getByteTransform().TransBool(resultError.Content, 0, 13); int alarmCount = 0; List alarmList = new ArrayList<>(); for (boolean alarmSign : statusAlarmList){ alarmCount++; if (alarmSign){ alarmList.add(alarmCount); } } alarmChangeSign = new HashSet<>(alarmList).equals(new HashSet<>(rgvProtocol.getAlarmList())); rgvProtocol.setAlarmList(alarmList); // rgvProtocol.setRgvPos((long) NumUtils.GetRandomIntInRange(1737000)); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] <<<<< 实时数据更新成功",DateUtils.convert(new Date()), slave.getId())); try { // 根据实时信息更新数据库 BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class); BasRgv basRgv = new BasRgv(); basRgv.setRgvNo(slave.getId()); basRgv.setRgvSts((int)rgvProtocol.getMode()); basRgv.setLoaded2(rgvProtocol.getLoaded2().intValue()); if (!basRgvService.updateById(rgvProtocol.toSqlModel(basRgv))){ log.error("RGV plc数据库更新失败 ===>> [id:{}] [ip:{}] [port:{}] [rack:{}] [slot:{}]", slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot()); } try{ if (!alarmChangeSign && !alarmList.isEmpty()){ BasRgvErrLogService basRgvErrLogService = SpringUtils.getBean(BasRgvErrLogService.class); BasRgvErrLog basRgvErrLog = new BasRgvErrLog(rgvProtocol.getAlarmList(), rgvProtocol.getTaskNo1(), rgvProtocol.getRgvNo()); basRgvErrLogService.save(basRgvErrLog); } } catch (Exception e){ log.error("RGV异常信息保存失败！！"); } } catch (Exception ignore) { } } else { initRgv(); OutputQueue.RGV.offer(MessageFormat.format("【{0}】读取RGV plc状态信息失败 ===>> [id:{1}] [ip:{2}] [port:{3}] [rack:{4}] [slot:{5}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot())); log.error("读取RGV plc状态信息失败 ===>> [id:{}] [ip:{}] [port:{}] [rack:{}] [slot:{}]", slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot()); } } catch (Exception e) { e.printStackTrace(); OutputQueue.RGV.offer(MessageFormat.format("【{0}】读取RGV plc状态信息失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort())); log.error("读取RGV plc状态信息失败 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); initRgv(); } } /** * 工位1写入数据 */ private void rgvOpt(RgvCommand command) { try{ BasRgvOptService basRgvOptService = SpringUtils.getBean(BasRgvOptService.class); BasRgvOpt basRgvOpt = new BasRgvOpt(rgvProtocol.getTaskNo1().intValue(), rgvProtocol.getRgvNo(), rgvProtocol.getRgvPosInt(), command); log.info(rgvProtocol.getRgvNo()+"号小车写入命令定位值："+rgvProtocol.getRgvPosInt()); basRgvOptService.save(basRgvOpt); }catch (Exception e){ log.error("RGV写入命令保存失败！！"); } } private void rgvOpt(Long command) { try{ BasRgvOptService basRgvOptService = SpringUtils.getBean(BasRgvOptService.class); BasRgvOpt basRgvOpt = new BasRgvOpt(rgvProtocol.getTaskNo1().intValue(), rgvProtocol.getRgvNo(), rgvProtocol.getRgvPosI(), command); basRgvOptService.save(basRgvOpt); }catch (Exception e){ log.error("RGV写入命令保存失败！！"); } } private boolean write(RgvCommand command) throws InterruptedException { if (null == command) { log.error("RGV写入命令为空"); return false; } siemensNet.Write("DB24.10.0", false); siemensNet.Write("DB24.10.1", false); command.setRgvNo(slave.getId()); short[] array = new short[5]; array[0] = command.getRgvNo().shortValue(); array[1] = command.getSourceStaNo1(); array[2] = command.getDestinationStaNo1(); array[3] = command.getTaskMode1();//任务模式 array[4] = command.getTaskNo1(); OperateResult result = siemensNet.Write("DB24.0", array); if (command.getAckFinish1().equals((short)0)) { // Thread.sleep(100L); siemensNet.Write("DB24.10.7", command.getRgvSome() == 1); Thread.sleep(20L); result = siemensNet.Write("DB24.10.0", true); } else { siemensNet.Write("DB24.10.1", true); } try { // 日志记录 BasRgvOptService bean = SpringUtils.getBean(BasRgvOptService.class); BasRgvOpt basRgvOpt = new BasRgvOpt( command.getTaskNo1().intValue(), command.getTaskNo2().intValue(), command.getRgvNo(), new Date(), command.getTaskModeType1().toString(), command.getSourceStaNo1().intValue(), command.getDestinationStaNo1().intValue(), command.getSourceStaNo2().intValue(), command.getDestinationStaNo2().intValue(), null, new Date(), null ); bean.save(basRgvOpt); } catch (Exception ignore) {} if (result != null && result.IsSuccess) { // Thread.sleep(200); // this.readStatus(); log.info("RGV 工位1命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSONString(command)); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 工位1命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSONString(command))); return true; } else { OutputQueue.RGV.offer(MessageFormat.format("【{0}】写入RGV plc工位1数据失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort())); log.error("写入RGV plc工位1数据失败 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); return false; } } private boolean write4(RgvCommand command) throws InterruptedException { if (null == command) { log.error("RGV写入命令为空"); return false; } // siemensNet.Write("DB24.10.0", false); // siemensNet.Write("DB24.10.1", false); command.setRgvNo(slave.getId()); short[] array = new short[5]; array[0] = command.getRgvNo().shortValue(); array[1] = command.getSourceStaNo1(); array[2] = command.getDestinationStaNo1(); array[3] = command.getTaskMode1();//任务模式 array[4] = command.getTaskNo1(); OperateResult result = siemensNet.Write("DB24.0", array); // // if (command.getAckFinish1().equals((short)0)) { //// Thread.sleep(100L); //// siemensNet.Write("DB24.10.7", command.getRgvSome() == 1); //// Thread.sleep(20L); //// result = siemensNet.Write("DB24.10.0", true); // } else { // siemensNet.Write("DB24.10.1", true); // } try { // 日志记录 BasRgvOptService bean = SpringUtils.getBean(BasRgvOptService.class); BasRgvOpt basRgvOpt = new BasRgvOpt( command.getTaskNo1().intValue(), command.getTaskNo2().intValue(), command.getRgvNo(), new Date(), command.getTaskModeType1().toString(), command.getSourceStaNo1().intValue(), command.getDestinationStaNo1().intValue(), command.getSourceStaNo2().intValue(), command.getDestinationStaNo2().intValue(), null, new Date(), null ); bean.save(basRgvOpt); } catch (Exception ignore) {} if (result != null && result.IsSuccess) { // Thread.sleep(200); // this.readStatus(); log.info("RGV 工位1取消命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSONString(command)); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 工位1命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSONString(command))); return true; } else { OutputQueue.RGV.offer(MessageFormat.format("【{0}】写入RGV plc工位1数据失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort())); log.error("写入RGV plc工位1取消数据失败 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); return false; } } /** * 完成 */ private void taskComplete() { try { OperateResultExOne result = siemensNet.Read("DB24.11", (short) 1); boolean[] status = siemensNet.getByteTransform().TransBool(result.Content, 0, 1); if (status[0]){ OperateResult result4 = siemensNet.Write("DB24.11.0", false); } } catch (Exception e) { log.error("RGV数据任务下发复位线程异常！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); } } private void write5(Long devperimeter){ try { siemensNet.Write("DB24.12", devperimeter.intValue()); Thread.sleep(10L); siemensNet.Write("DB24.11.0", true); } catch (Exception ignore) { log.error("写入RGV plc工位1漫游数据失败 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); } } private boolean write3(RgvCommand command) throws InterruptedException { if (null == command) { log.error("RGV写入命令为空"); return false; } siemensNet.Write("DB24.10.0", false); siemensNet.Write("DB24.10.1", false); command.setRgvNo(slave.getId()); short[] array = new short[5]; array[0] = command.getRgvNo().shortValue(); array[1] = command.getSourceStaNo1(); array[2] = command.getDestinationStaNo1(); array[3] = command.getTaskMode1();//任务模式 array[4] = command.getTaskNo1(); // OperateResult result = siemensNet.Write("DB24.0", array); OperateResult result = null; if (command.getAckFinish1().equals((short)0)) { Thread.sleep(20L); result = siemensNet.Write("DB24.10.0", true); } else { siemensNet.Write("DB24.10.1", true); } try { // 日志记录 BasRgvOptService bean = SpringUtils.getBean(BasRgvOptService.class); BasRgvOpt basRgvOpt = new BasRgvOpt( command.getTaskNo1().intValue(), command.getTaskNo2().intValue(), command.getRgvNo(), new Date(), command.getTaskModeType1().toString(), command.getSourceStaNo1().intValue(), command.getDestinationStaNo1().intValue(), command.getSourceStaNo2().intValue(), command.getDestinationStaNo2().intValue(), null, new Date(), null ); bean.save(basRgvOpt); } catch (Exception ignore) {} if (result != null && result.IsSuccess) { // Thread.sleep(200); // this.readStatus(); log.info("RGV 工位1命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSONString(command)); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 工位1命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSONString(command))); return true; } else { OutputQueue.RGV.offer(MessageFormat.format("【{0}】写入RGV plc工位1数据失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort())); log.error("写入RGV plc工位1数据失败 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort()); return false; } } @Override public void close() { if (siemensNet != null) { siemensNet.ConnectClose(); } } // ==================== 模拟模式方法 ==================== /** * 模拟完成确认（step=1）：模拟模式无需读PLC */ private void taskCompleteCS() { // 模拟模式下无PLC，跳过 } /** * 模拟取放货下发（step=2）：记录到simTask，启动模拟移动 * 取放货(mode=3): 第一阶段去取货站，到达后等待5秒，再去放货站 */ private void writeCS(RgvCommand command) { if (command == null) return; SimTask st = new SimTask(); st.setTaskNo(command.getTaskNo1()); st.setSourceSta(command.getSourceStaNo1()); st.setDestSta(command.getDestinationStaNo1()); st.setMode(command.getTaskMode1()); st.setPhase(SimPhase.MOVING); st.setStartPos(rgvProtocol.getRgvPos() != null ? rgvProtocol.getRgvPos() : 0L); if (command.getTaskMode1().intValue() == RgvTaskModeType.FETCH_PUT.id) { // 取放货：先去取货站，记下放货站位置 st.setTargetPos(getStationPosition(command.getSourceStaNo1())); st.setDestPos(getStationPosition(command.getDestinationStaNo1())); } else { // 其他模式：直接去目标站 st.setTargetPos(getStationPosition(command.getDestinationStaNo1())); st.setDestPos(0L); } simTasks.put(slave.getId(), st); log.info("[模拟] RGV{} 取放货任务接收: taskNo={} source={} dest={} 目标位置={}", slave.getId(), command.getTaskNo1(), command.getSourceStaNo1(), command.getDestinationStaNo1(), st.getTargetPos()); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 取放货任务接收: source={2} dest={3} target={4}", DateUtils.convert(new Date()), slave.getId(), command.getSourceStaNo1(), command.getDestinationStaNo1(), st.getTargetPos())); } /** * 模拟复位（step=3）：清除模拟任务 */ private void write3CS(RgvCommand command) { simTasks.remove(slave.getId()); rgvProtocol.setTaskNo1((short) 0); rgvProtocol.setStatus((short) 0); log.info("[模拟] RGV{} 复位完成", slave.getId()); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 复位完成", DateUtils.convert(new Date()), slave.getId())); } /** * 模拟取消（step=4）：清除模拟任务 */ private void write4CS(RgvCommand command) { simTasks.remove(slave.getId()); log.info("[模拟] RGV{} 任务取消: taskNo={}", slave.getId(), command.getTaskNo1()); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 任务取消: taskNo={2}", DateUtils.convert(new Date()), slave.getId(), command.getTaskNo1())); } /** * 模拟漫游下发（step=5）：记录到simTask，启动模拟移动 */ private void write5CS(Long destination) { SimTask st = new SimTask(); st.setMode((short) 5); st.setPhase(SimPhase.MOVING); st.setTargetPos(destination); st.setStartPos(rgvProtocol.getRgvPos() != null ? rgvProtocol.getRgvPos() : 0L); simTasks.put(slave.getId(), st); log.info("[模拟] RGV{} 漫游任务接收: 目标位置={}", slave.getId(), destination); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 漫游任务接收: 目标={2}", DateUtils.convert(new Date()), slave.getId(), destination)); } // ==================== 模拟运行线程 ==================== /** * 模拟运行线程：每200ms更新本车位置 + 检测避让（各线程只处理自己的simTask） */ private void simThreadRun() { log.info("[模拟] RGV{} 模拟运行线程启动", slave.getId()); int tick = 0; int myId = slave.getId(); while (true) { try { Thread.sleep(200); tick++; SimTask st = simTasks.get(myId); if (st == null || st.getPhase() == SimPhase.IDLE) continue; if (rgvProtocol == null || rgvProtocol.getRgvPos() == null) continue; long current = rgvProtocol.getRgvPos(); long target = st.getTargetPos(); double dist = forwardDistance(current, target); // 到达取货站：等待5秒后前往放货站 if (st.getPhase() == SimPhase.WAITING_AT_SOURCE) { long waited = System.currentTimeMillis() - st.getWaitStartTime(); if (waited >= 5000) { st.setTargetPos(st.getDestPos()); st.setStartPos(current); st.setPhase(SimPhase.MOVING); log.info("[模拟] RGV{} 等待{}ms结束，开始前往放货站: pos={}", myId, waited, st.getDestPos()); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 等待结束，前往放货站: {2}", DateUtils.convert(new Date()), myId, st.getDestPos())); } continue; } if (dist <= SIM_CS_MOVE_STEP) { // 到达目标 rgvProtocol.setRgvPos(target); rgvProtocol.setRgvPosInt((int) target); if (st.getMode() == RgvTaskModeType.FETCH_PUT.id.shortValue() && st.getDestPos() != 0L && target != st.getDestPos()) { // 阶段1：到达取货站，进入等待 rgvProtocol.setStatus((short) 2); // 进站运行中 st.setPhase(SimPhase.WAITING_AT_SOURCE); st.setWaitStartTime(System.currentTimeMillis()); log.info("[模拟] RGV{} 到达取货站: pos={}，等待5秒后前往放货站", myId, target); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 到达取货站: {2}，等待5秒", DateUtils.convert(new Date()), myId, target)); } else { // 阶段2到达放货站 / 漫游到达 → 申请完成任务 rgvProtocol.setStatus((short) 100); // WAITING simTasks.remove(myId); log.info("[模拟] RGV{} 到达放货站: pos={}，状态变为申请完成(WAITING)", myId, target); OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 到达放货站: {2}，申请完成任务", DateUtils.convert(new Date()), myId, target)); } try { BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class); basRgvService.updateById(rgvProtocol.toSqlModel(new BasRgv())); } catch (Exception ignore) {} } else { // 前进（减速逻辑：前方有车则降低步长，不重合不超过） long step = calcAdjustedStep(myId, current); long newPos = (current + step) % PERIMETER; rgvProtocol.setRgvPos(newPos); rgvProtocol.setRgvPosInt((int) newPos); rgvProtocol.setStatus((short) 11); // ROAM if (tick % 5 == 0) { log.info("[模拟] RGV{} 移动中: {} → {} (距目标{}, 步长={})", myId, current, newPos, (long) dist, step); } try { BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class); basRgvService.updateById(rgvProtocol.toSqlModel(new BasRgv())); } catch (Exception ignore) {} // 检测避让：只检查本车前方是否有阻挡 checkAndAvoidBlocker(myId, target); } } catch (Exception e) { log.error("[模拟] RGV{} 模拟线程异常: {}", slave.getId(), e.getMessage()); } } } /** * 检测前方阻挡并递推避让 * @param myRgvNo 当前小车编号 * @param myTarget 当前小车的目标位置 */ private void checkAndAvoidBlocker(int myRgvNo, long myTarget) { RgvThread myThread = getRgvThread(myRgvNo); if (myThread == null || myThread.getRgvProtocol() == null) return; long myPos = myThread.getRgvProtocol().getRgvPos(); if (myPos == 0) return; double distToTarget = forwardDistance(myPos, myTarget); if (distToTarget <= 0) return; try { BasCircularShuttleService shuttleService = SpringUtils.getBean(BasCircularShuttleService.class); List shuttles = shuttleService.list( new QueryWrapper().orderByAsc("rgv_id")); if (shuttles == null) return; // 只找离自己最近的前车（在自己与目标之间） int nearestRgvNo = -1; double nearestDist = Double.MAX_VALUE; for (BasCircularShuttle shuttle : shuttles) { int otherNo = shuttle.getRgvNo(); if (otherNo == myRgvNo) continue; if (!isIdleForAvoidance(otherNo)) continue; RgvThread otherThread = getRgvThread(otherNo); if (otherThread == null || otherThread.getRgvProtocol() == null) continue; long otherPos = otherThread.getRgvProtocol().getRgvPos(); if (otherPos == 0) continue; double fwdDist = forwardDistance(myPos, otherPos); // 对方在前方 且 在目标点之前 → 阻挡 if (fwdDist > 0 && fwdDist < distToTarget) { if (fwdDist < nearestDist) { nearestDist = fwdDist; nearestRgvNo = otherNo; } } } if (nearestRgvNo > 0) { long avoidDest = forwardAvoidDest(myTarget); issueAvoidanceRecursive(nearestRgvNo, avoidDest); } } catch (Exception ignore) {} } /** * 判断RGV是否空闲可用于避让 */ private boolean isIdleForAvoidance(int rgvNo) { SimTask otherSim = simTasks.get(rgvNo); if (otherSim != null && otherSim.getPhase() == SimPhase.MOVING) return false; RgvThread otherThread = getRgvThread(rgvNo); if (otherThread == null) return false; RgvProtocol protocol = otherThread.getRgvProtocol(); if (protocol == null) return false; int st = protocol.getStatus() != null ? protocol.getStatus() : -1; return (st == 0 || st == 11) && protocol.getTaskNo1() == 0; } /** * 下发避让：给rgvNo写入避让simTask至avoidDest * 该RGV的线程会在下一tick自行检测避让路径上的阻挡 */ private void issueAvoidanceRecursive(int rgvNo, long avoidDest) { MessageQueue.offer(SlaveType.Rgv, rgvNo, new Task(5, avoidDest)); // RgvThread otherThread = getRgvThread(rgvNo); // if (otherThread == null || otherThread.getRgvProtocol() == null) return; // long currentPos = otherThread.getRgvProtocol().getRgvPos(); // // // 写入simTask（覆盖已有的WAITING_AT_SOURCE或MOVING任务） // SimTask st = new SimTask(); // st.setMode((short) 5); // st.setPhase(SimPhase.MOVING); // st.setTargetPos(avoidDest); // st.setStartPos(currentPos); // simTasks.put(rgvNo, st); // // log.info("[模拟避让] RGV{} 前方RGV{}阻挡，避让至: {}", // slave.getId(), rgvNo, avoidDest); // OutputQueue.RGV.offer(MessageFormat.format( // "【{0}】[模拟避让] RGV{1} 前方RGV{2}阻挡, 避让至: {3}", // DateUtils.convert(new Date()), slave.getId(), rgvNo, avoidDest)); } /** * 计算目标点前方10000处的避让位置 */ private long forwardAvoidDest(long target) { long dest = target + 100000; if (dest > PERIMETER) dest -= PERIMETER; return dest; } /** 前车跟随距离：进入此范围开始减速 */ private static final long FOLLOW_DISTANCE = 20000; /** 最小安全间距：不能比这更近 */ private static final long MIN_GAP = 3000; /** * 根据前方车辆距离计算本步实际步长 * 前方无车 → 全速；有车 → 线性减速；贴紧 → 停止 */ private long calcAdjustedStep(int myId, long myPos) { long aheadDist = distanceToNearestAhead(myId, myPos); if (aheadDist < 0) return SIM_CS_MOVE_STEP; // 前方无车，全速 long gap = aheadDist - MIN_GAP; if (gap <= 0) return 0; // 已到最小间距，停止 return Math.min(SIM_CS_MOVE_STEP, gap); } /** * 查找正前方最近一辆车的正向距离，无车返回-1 * 只关注有任务或正在移动的车，空闲静止的车由避让逻辑处理 */ private long distanceToNearestAhead(int myId, long myPos) { long best = -1; try { BasCircularShuttleService shuttleService = SpringUtils.getBean(BasCircularShuttleService.class); List shuttles = shuttleService.list( new QueryWrapper().orderByAsc("rgv_id")); if (shuttles == null) return -1; for (BasCircularShuttle shuttle : shuttles) { int otherId = shuttle.getRgvNo(); if (otherId == myId) continue; SimTask otherTask = simTasks.get(otherId); // 只关注有任务的车（MOVING / WAITING_AT_SOURCE） if (otherTask == null || otherTask.getPhase() == SimPhase.IDLE) continue; RgvThread otherThread = getRgvThread(otherId); if (otherThread == null || otherThread.getRgvProtocol() == null) continue; Long otherPos = otherThread.getRgvProtocol().getRgvPos(); if (otherPos == null || otherPos == 0) continue; double fwd = forwardDistance(myPos, otherPos); if (fwd > 0 && fwd <= FOLLOW_DISTANCE && (best < 0 || fwd < best)) { best = (long) fwd; } } } catch (Exception ignore) {} return best; } /** * 计算环形轨道上从current到target的正向距离 */ private double forwardDistance(long current, long target) { if (target >= current) return target - current; return PERIMETER - current + target; } /** * 获取站点在轨道上的位置 */ private long getStationPosition(short staNo) { try { BasDevpPositionService positionService = SpringUtils.getBean(BasDevpPositionService.class); com.zy.asrs.entity.BasDevpPosition pos = positionService.getOne( new QueryWrapper().eq("dev_no", (int) staNo)); if (pos != null && pos.getPlcPosition() != null) return pos.getPlcPosition(); } catch (Exception ignore) {} return 0L; } /** * 获取指定RGV的RgvThread实例 */ private RgvThread getRgvThread(int rgvNo) { try { return (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvNo); } catch (Exception e) { return null; } } /******************************************************************************************/ /**************************************** 测试专用 *****************************************/ /*****************************************************************************************/ // public static void main(String[] args) throws InterruptedException { // RgvSlave slave = new RgvSlave(); // slave.setId(1); // slave.setIp("192.168.6.9"); // slave.setRack(0); // slave.setSlot(0); // RgvThread rgvThread = new RgvThread(slave); // rgvThread.connect(); // rgvThread.readStatus(); // System.out.println(JSON.toJSONString(rgvThread.rgvProtocol)); // Thread.sleep(3000L); // // } }