*

L

2026-04-23 702549f5d846ae76cc005c56d03fd774a3caa341

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

@@ -5,18 +5,24 @@
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.asrs.utils.RouteUtils;
import com.zy.core.ThreadHandler;
import com.zy.core.cache.MessageQueue;
import com.zy.core.cache.OutputQueue;
import com.zy.core.enums.RgvStatusType;
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;
@@ -27,7 +33,8 @@
import lombok.extern.slf4j.Slf4j;

import java.text.MessageFormat;
import java.util.Date;
import java.util.*;
import java.util.concurrent.ConcurrentHashMap;

/**
 * RGV线程
@@ -49,6 +56,35 @@
     */
    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<Integer, SimTask> simTasks = new ConcurrentHashMap<>();

    public RgvThread(RgvSlave slave) {
        this.slave = slave;
@@ -57,14 +93,10 @@
    @Override
    @SuppressWarnings("InfiniteLoopStatement")
    public void run() {
        connectRgv = this.connect();
        while(!connectRgv){
            try {
                connectRgv = this.connect();
                Thread.sleep(100);
            } catch (Exception e){

            }
        if (csSign) {
            connectRgv = true; // 模拟模式跳过PLC连接
        } else {
            connectRgv = this.connect();
        }

        // 启动线程自动重连
@@ -75,6 +107,11 @@

        // 启动任务下发线程
        new Thread(this::taskIssued).start();

        // 模拟模式：启动模拟运行线程
        if (csSign) {
            new Thread(this::simThreadRun).start();
        }
    }


@@ -84,52 +121,104 @@
    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();
                }
                switch (step) {
                    // 读数据
                    case 1:
                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();
                        break;
                    //工位1写入数据
                    case 2:
                        write((RgvCommand) task.getData());
                        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);     // 目标站
                        write3(command);
                        break;
                    // 复位
                    case 4:
                        RgvCommand command4 = (RgvCommand) task.getData();
                        if (null == command4) {
                            command4 = new RgvCommand();
                        }
                        command4.setRgvNo(slave.getId()); // RGV编号
                        command4.setTaskNo1((short) 0); // 工作号
                        command4.setAckFinish1((short) 1);  // 任务完成确认位
                        command4.setTaskMode1(RgvTaskModeType.X_MOVE); // 任务模式
                        command4.setSourceStaNo1((short)0);     // 源站
                        command4.setDestinationStaNo1((short)0);     // 目标站
                        write3(command4);
                        break;
                    default:
                        break;
                            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(100);
                Thread.sleep(50);
            } catch (Exception e) {
                log.error("RGV写线程异常"+e.getMessage());
//                e.printStackTrace();
@@ -143,6 +232,16 @@
            try {
                Thread.sleep(1000);
                if(!connectRgv){
                    try {
                        // 根据实时信息更新数据库
                        BasCircularShuttleService basCircularShuttleService = SpringUtils.getBean(BasCircularShuttleService.class);
                        BasCircularShuttle basCircularShuttle = basCircularShuttleService.getOne(new QueryWrapper<BasCircularShuttle>().eq("rgv_no",  slave.getId()));
                        if (basCircularShuttle.getStatus() != 0){
                            continue;
                        }
                    } catch (Exception ignore) {

                    }
                    try {
                        connectRgv = this.connect();
                        Thread.sleep(100);
@@ -162,9 +261,18 @@
    private void readStatusRgv() {
        while (true) {
            try {
                Thread.sleep(40);
                readStatus();
                if(!connectRgv){
                    try {
                        Thread.sleep(1000L);
                    } catch (Exception e){

                    }
                    initRgv();
                    continue;
                }
                Thread.sleep(40);
//                readStatus();
                initRgv();
            } catch (Exception e) {
                log.error("RGV读线程异常"+e.getMessage());

@@ -181,6 +289,17 @@
     * 初始化RGV状态
     */
    private void initRgv() {
       if (csSign){
           initRgvCS();
       } else {
           initRgvT();
       }
    }

    /**
     * 初始化RGV状态
     */
    private void initRgvT() {
        if (null == rgvProtocol) {
            rgvProtocol = new RgvProtocol();
        }
@@ -199,6 +318,64 @@
        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
@@ -229,6 +406,7 @@
            OperateResultExOne<byte[]> result = siemensNet.Read("DB100.0", (short) 20);
            OperateResultExOne<byte[]> resultV = siemensNet.Read("DB20.16", (short) 2);
            OperateResultExOne<byte[]> resultE = siemensNet.Read("DB20.26", (short) 2);
            OperateResultExOne<byte[]> resultError = siemensNet.Read("DB13.0", (short) 13);
            if (result.IsSuccess && resultV.IsSuccess && resultE.IsSuccess) {
                if (null == rgvProtocol) {
                    rgvProtocol = new RgvProtocol();
@@ -242,11 +420,29 @@
                rgvProtocol.setAlarm(siemensNet.getByteTransform().TransInt16(result.Content, 10));
                rgvProtocol.setStatus(siemensNet.getByteTransform().TransInt16(result.Content, 12));
                rgvProtocol.setxSpeed(siemensNet.getByteTransform().TransInt16(result.Content, 14));
                rgvProtocol.setRgvPos(siemensNet.getByteTransform().TransUInt32(result.Content, 16));
                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));
//                rgvProtocol.setRgvPos((long)NumUtils.GetRandomIntInRange(1737000));
                boolean[] statusAlarmList = siemensNet.getByteTransform().TransBool(resultError.Content, 0, 13);
                int alarmCount = 0;
                List<Integer> 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 {
                    // 根据实时信息更新数据库
@@ -258,6 +454,16 @@
                    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) {

                }
@@ -278,6 +484,25 @@
    /**
     * 工位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写入命令为空");
@@ -325,7 +550,7 @@
                    new Date(),
                    null
            );
            bean.insert(basRgvOpt);
            bean.save(basRgvOpt);
        } catch (Exception ignore) {}

        if (result != null && result.IsSuccess) {
@@ -340,6 +565,95 @@
            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<byte[]> 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写入命令为空");
@@ -383,7 +697,7 @@
                    new Date(),
                    null
            );
            bean.insert(basRgvOpt);
            bean.save(basRgvOpt);
        } catch (Exception ignore) {}

        if (result != null && result.IsSuccess) {
@@ -401,24 +715,384 @@

    @Override
    public void close() {
        siemensNet.ConnectClose();
        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<BasCircularShuttle> shuttles = shuttleService.list(
                    new QueryWrapper<BasCircularShuttle>().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<BasCircularShuttle> shuttles = shuttleService.list(
                    new QueryWrapper<BasCircularShuttle>().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<com.zy.asrs.entity.BasDevpPosition>().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);

    }
//    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);
//
//    }

}