zy-wcs.git - Gitblit

			@@ -5,40 +5,34 @@
			import HslCommunication.Profinet.Siemens.SiemensPLCS;
			import HslCommunication.Profinet.Siemens.SiemensS7Net;
			import com.alibaba.fastjson.JSON;
			import com.baomidou.mybatisplus.mapper.EntityWrapper;
			import com.core.common.Cools;
			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.BasRgv;
			import com.zy.asrs.entity.BasRgvMap;
			import com.zy.asrs.entity.BasRgvOpt;
			import com.zy.asrs.service.BasDevpService;
			import com.zy.asrs.service.BasRgvMapService;
			import com.zy.asrs.service.BasRgvOptService;
			import com.zy.asrs.service.BasRgvService;
			import com.zy.common.utils.News;
			import com.zy.core.RgvThread2;
			import com.zy.asrs.service.DeviceErrorService;
			import com.zy.asrs.utils.RouteUtils;
			import com.zy.asrs.utils.TrackRangeUtils;
			import com.zy.core.DevpThread;
			import com.zy.core.ThreadHandler;
			import com.zy.core.cache.MessageQueue;
			import com.zy.core.cache.OutputQueue;
			import com.zy.core.cache.*;
			import com.zy.core.enums.RgvModeType;
			import com.zy.core.enums.RgvStatusType;
			import com.zy.core.enums.RgvTaskModeType;
			import com.zy.core.enums.RgvTaskStatusType;
			import com.zy.core.enums.SlaveType;
			import com.zy.core.model.RgvSlave;
			import com.zy.core.model.Task;
			import com.zy.core.model.command.CrnCommand;
			import com.zy.core.model.command.RgvCommand;
			import com.zy.core.model.protocol.RgvProtocol;
			import com.zy.core.model.protocol.StaProtocol;
			import com.zy.core.model.protocol.*;
			import lombok.Data;
			import lombok.extern.slf4j.Slf4j;
			import org.springframework.beans.factory.annotation.Value;

			import java.sql.Wrapper;
			import java.text.MessageFormat;
			import java.util.*;
			import java.util.Date;
			import java.util.List;
			import java.util.concurrent.ConcurrentHashMap;

			/**
			@@ -47,12 +41,19 @@
			*/
			@Data
			@Slf4j
			public class RgvThread implements Runnable, RgvThread2 {
			public class RgvThread implements Runnable, ThreadHandler {

			private SiemensS7Net siemensNet;
			private RgvSlave slave;
			private RgvProtocol rgvProtocol;
			private Map<Integer, StaProtocol> station = new ConcurrentHashMap<>();
			// private RgvProtocol rgvProtocol;
			private TaskProtocolCache taskProtocolCache = new TaskProtocolCache();
			// # 轨道总长
			private Long trackEntireLength = 240000L;
			//# 轨道基准点
			private Long trackBenchmark = 1L;
			// # 避让距离
			private Long avoidDistance = 11200L;
			private String errorRgv = "-";

			/**
			* 工位1复位信号
			@@ -63,20 +64,11 @@
			*/
			private boolean resetFlag2 = false;

			//锁定标记
			private boolean PakMk = true;
			//入库标记
			// private boolean PakIn = true;
			// //出库标记
			// private boolean PakOut = true;
			// //根据距离跳过取货
			// private boolean PakRgv = true;
			// //接驳标记
			// private boolean PakToCrn = true;
			// 任务锁定
			private boolean Paking = true;
			// //连续任务下发
			// private boolean PakAll = true;
			private boolean connectRgv = false;
			public Long currentTimeMilliConnectRgv= 0L;

			private boolean delRgvTask = false;
			private short wrkSign = 0;

			public RgvThread(RgvSlave slave) {
			this.slave = slave;
			@@ -85,132 +77,934 @@
			@Override
			@SuppressWarnings("InfiniteLoopStatement")
			public void run() {
			this.connect();
			initRgv();

			// 启动线程自动重连
			new Thread(this::rgvConnect).start();

			// 启动读数据线程
			new Thread(this::readStatusRgv).start();

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

			// 启动漫游线程
			new Thread(this::taskWalkIssued).start();
			// new Thread(this::taskWalkIssued2).start();

			// 启动任务完成线程
			new Thread(this::taskComplete).start();
			}

			private void rgvConnect() {
			while (true) {
			try {
			int step = 1;
			Task task = MessageQueue.poll(SlaveType.Rgv, slave.getId());
			if (task != null) {
			step = task.getStep();
			Thread.sleep(1000);
			if(!connectRgv){
			try {
			connectRgv = this.connect();
			Thread.sleep(100);
			} catch (Exception e){

			}
			}
			switch (step) {
			// 读数据
			case 1:
			readStatus();
			break;
			// 小车工位写入数据
			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.setTaskNo(0); // 工作号
			command.setTaskStatus(RgvTaskStatusType.NONE); // 任务模式
			command.setTargetPosition(0); // 源站
			command.setWrkTaskPri(0); // 目标站
			command.setCommand(false);
			write2(command);
			break;
			default:
			break;
			}
			Thread.sleep(500);
			} catch (Exception e) {
			log.error("rgv连接失败！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "rgv连接失败"+e.getMessage());
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			initRgv();
			// e.printStackTrace();
			}
			}
			}

			private void readStatusRgv() {
			while (true) {
			try {
			if(!connectRgv){
			try {
			Thread.sleep(1000L);
			} catch (Exception e){

			}
			initRgv();
			continue;
			}
			Thread.sleep(100L);
			// System.out.println("读线程,小车号"+ slave.getId()+"时间戳："+System.currentTimeMillis());

			readStatus();

			} catch (Exception e) {
			log.error("RGV数据读取线程异常！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV数据读取线程异常"+e.getMessage());
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			initRgv();
			// e.printStackTrace();
			}

			}

			}
			private static final Set<Integer> specialSites = new HashSet<>(
			Arrays.asList(1004,1014,1018,1028,1035));
			public static final Map<Integer, Integer> StaPosition = new HashMap<Integer, Integer>() {{
			put(1004, 1001);put(1014, 1011);put(1018, 1015);put(1028, 1025);put(1035, 1032);

			}};
			public static final ArrayList<Integer> staNos1 = new ArrayList<Integer>() {{
			add(1);add(2);

			}};

			public static final ArrayList<Integer> staNos2 = new ArrayList<Integer>() {{
			add(3);add(4);

			}};
			/**
			* 初始化站点状态
			* 完成
			*/
			private void initSite() {
			ArrayList<Integer> staNos = getStaNo();
			private void taskComplete() {
			while (true) {
			try {
			if(!connectRgv){
			try {
			Thread.sleep(1000L);
			} catch (Exception e){

			// 站点编号
			for (Integer siteId : staNos) {
			StaProtocol staProtocol = station.get(siteId);
			if (null == staProtocol) {
			staProtocol = new StaProtocol();
			staProtocol.setSiteId(siteId);
			station.put(siteId, staProtocol);
			}
			continue;
			}
			staProtocol.setWorkNo(0); // ID
			staProtocol.setAutoing(false); // 自动
			staProtocol.setLoading(false); // 有物
			staProtocol.setInEnable(false); // 可入
			staProtocol.setOutEnable(false); // 可出
			staProtocol.setEmptyMk(false); // 空板信号
			staProtocol.setStaNo((short) 0); // 目标站
			if (delRgvTask){
			writeDelRgvTask();
			delRgvTask = false;
			continue;
			}
			Thread.sleep(50L);

			// 构建设备状态对象
			RgvProtocol rgvProtocol = RgvStatusCache.getRgvStatus(slave.getId());
			if (rgvProtocol == null) {
			rgvProtocol = new RgvProtocol();
			}
			if (rgvProtocol.getStatusType().equals(RgvStatusType.WAITING) \|\| rgvProtocol.getStatusType().equals(RgvStatusType.FETCHWAITING)){
			boolean sign = true;
			int i = 1;
			while (sign && i<5){
			i++;
			sign = false;
			OperateResult result2 = siemensNet.Write("DB100.2",(int) 0);
			OperateResult result14 = siemensNet.Write("DB100.14", (int) 0);
			OperateResult result6 = siemensNet.Write("DB100.6", (short) 0);
			OperateResult result8 = siemensNet.Write("DB100.8", (short) 0);
			OperateResult result10 = siemensNet.Write("DB100.10", (short) 0);
			OperateResult result12 = siemensNet.Write("DB100.12", (short) 0);
			Thread.sleep(10L);

			OperateResultExOne<byte[]> resultRead0 = siemensNet.Read("DB100.0", (short) 20);
			Integer i2 = siemensNet.getByteTransform().TransInt32(resultRead0.Content, 2);
			short i6 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 6);
			short i8 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 8);
			short i10 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 10);
			Integer i14 = siemensNet.getByteTransform().TransInt32(resultRead0.Content, 14);
			if (i2!=0 \|\| i6 != (short) 0
			\|\| i8!=(short)0 \|\| i10 != (short) 0
			\|\| i14!=0){
			sign = true;
			}
			}

			OperateResult result0 = siemensNet.Write("DB100.0", (short) 1);

			Thread.sleep(20L);

			OperateResult result00 = siemensNet.Write("DB100.0", (short) 0);
			}
			} catch (Exception e) {
			log.error("RGV数据任务下发复位线程异常！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV数据任务下发复位线程异常"+e.getMessage());
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			initRgv();
			// e.printStackTrace();
			}

			}

			}

			/**
			* 漫游
			*/
			private void taskWalkIssued() {
			while (true) {
			try {
			if(!connectRgv){
			try {
			Thread.sleep(1000L);
			} catch (Exception e){

			}
			continue;
			}
			// 休眠 1 秒
			Thread.sleep(200L);

			if (!deviceDetection()) {
			continue;
			}
			RgvTaskProtocol rgvTaskProtocol = RgvTaskCache.getRgvStatus(slave.getId());
			if (rgvTaskProtocol == null) {
			initRgv();
			rgvTaskProtocol = RgvTaskCache.getRgvStatus(slave.getId());
			}
			if (rgvTaskProtocol.getAvoid() != 1) {
			continue;
			}
			TaskProtocol issued = new TaskProtocol();
			issued.setTaskNo(32222L);
			issued.setTargetPosition(rgvTaskProtocol.getAvoidingTheDestination());
			issued.setTaskStatus(1);
			issued.setDirection(true);
			write(issued);
			rgvTaskProtocol.setAvoid(0);

			// Thread.sleep(200);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocol);

			} catch (Exception e) {
			log.error("RGV行走任务下发线程异常！！！" + e.getMessage());
			// e.printStackTrace();
			}
			}
			}

			/**
			* 任务下发
			*/
			private void taskIssued() {
			while (true) {
			if(!connectRgv){
			try {
			Thread.sleep(1000L);
			} catch (Exception e){

			}
			continue;
			}
			RgvRunProtocol rgvRun = RgvRunCache.getRgvRun();
			if (!rgvRun.getRgvNo().equals(slave.getId())){
			continue;
			}
			try {
			Thread.sleep(100);
			rgvRun = RgvRunCache.getRgvRun();
			// System.out.println(JSON.toJSON(rgvRun));

			// 休眠 1 秒
			if (System.currentTimeMillis() - rgvRun.currentTimeMilliOld > 500L) {
			rgvRun.currentTimeMilliOld = System.currentTimeMillis()+50;
			} else {
			continue;
			}
			if (!deviceDetection()) {
			rgvRun.currentTimeMilliOld = System.currentTimeMillis()+50;
			rgvRun.setRgvNo(slave.getOtherId());
			RgvRunCache.updateRgvStatus(rgvRun);
			if (!errorRgv.equals("无")){
			RgvErrCache.updateRgvErr(slave.getId(),errorRgv);
			}
			continue;
			}

			Thread.sleep(50L);

			RgvProtocol rgvProtocol = RgvStatusCache.getRgvStatus(slave.getId());
			RgvTaskProtocol rgvTaskProtocol = RgvTaskCache.getRgvStatus(slave.getId());
			if (rgvProtocol == null \|\| rgvTaskProtocol == null) {
			initRgv();
			rgvProtocol = RgvStatusCache.getRgvStatus(slave.getId());
			rgvTaskProtocol = RgvTaskCache.getRgvStatus(slave.getId());
			}
			if (!rgvProtocol.getStatusType().equals(RgvStatusType.IDLE) \|\| (Math.abs(rgvProtocol.getRgvPos() - rgvProtocol.getRgvPosDestination())>100)){
			rgvRun.currentTimeMilliOld = System.currentTimeMillis()+50;
			rgvRun.setRgvNo(slave.getOtherId());
			RgvRunCache.updateRgvStatus(rgvRun);
			continue;
			}
			if (rgvProtocol.getLoaded() == -1){
			rgvRun.currentTimeMilliOld = System.currentTimeMillis()+50;
			rgvRun.setRgvNo(slave.getOtherId());
			RgvRunCache.updateRgvStatus(rgvRun);
			RgvErrCache.updateRgvErr(slave.getId(),"小车探物物状态异常");
			continue;
			}
			if (rgvTaskProtocol.getAvoid() != 0) {
			rgvRun.currentTimeMilliOld = System.currentTimeMillis()+50;
			rgvRun.setRgvNo(slave.getOtherId());
			RgvRunCache.updateRgvStatus(rgvRun);
			continue;
			}

			List<TaskProtocol> allTakeTaskProtocol = taskProtocolCache.getTakeOrPutTaskProtocol(rgvProtocol.getLoaded());
			if (allTakeTaskProtocol.isEmpty()){
			allTakeTaskProtocol = taskProtocolCache.getAllWalkTaskProtocol();
			for (TaskProtocol taskProtocol : allTakeTaskProtocol) {
			if (taskProtocol.getIsRunning() == 1) {//准备下发
			// 双车
			if (rgvOtherStatusEnable()) {
			//另一台车是否允许此台车执行
			if (!otherRgvAvoid(taskProtocol.getTargetPosition())) {
			RgvErrCache.updateRgvErr(slave.getId(),errorRgv);
			break;
			}
			}
			if (taskProtocol.getTargetPosition()<=rgvProtocol.getRgvPos()+50
			&& taskProtocol.getTargetPosition()>=rgvProtocol.getRgvPos()-50){
			taskProtocolCache.removeTaskProtocol(taskProtocol.getTaskNoDirection());
			RgvErrCache.updateRgvErr(slave.getId());
			break;
			} else {
			TaskProtocol issued = new TaskProtocol(taskProtocol,true);
			write(issued);
			RgvErrCache.updateRgvErr(slave.getId());
			taskProtocolCache.removeTaskProtocol(taskProtocol.getTaskNoDirection());
			break;
			}
			}
			}
			RgvErrCache.updateRgvErr(slave.getId());
			} else {
			for (TaskProtocol taskProtocol : allTakeTaskProtocol) {
			if (taskProtocol.getIsRunning() == 1) {//准备下发
			//双车
			if (rgvOtherStatusEnable()) {
			//另一台车是否允许此台车执行
			if (!otherRgvAvoid(taskProtocol.getTargetPosition())) {
			RgvErrCache.updateRgvErr(slave.getId(),errorRgv);
			break;
			}
			}
			if (taskProtocol.getTargetPosition()<=rgvProtocol.getRgvPos()+50
			&& taskProtocol.getTargetPosition()>=rgvProtocol.getRgvPos()-50){
			if (taskProtocol.getTaskStatus()==3){
			DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, taskProtocol.getTargetPositionStaNoPlcId());
			StaProtocol staProtocol = devpThread.getStation().get(taskProtocol.getTargetPositionStaNo());
			if (staProtocol == null ) {
			RgvErrCache.updateRgvErr(slave.getId(),"未查到小车作业站"+staProtocol.getStaNo()+"站点");
			break;
			}
			// 判断是否满足入库条件
			if (!staProtocol.isAutoing() \|\| staProtocol.isLoading()
			){
			RgvErrCache.updateRgvErr(slave.getId(),"小车等待"+staProtocol.getStaNo()+"站点就绪");
			break;
			}
			} else if (taskProtocol.getTaskStatus()==2){
			DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, taskProtocol.getTargetPositionStaNoPlcId());
			StaProtocol staProtocol = devpThread.getStation().get(taskProtocol.getTargetPositionStaNo());
			if (staProtocol == null ) {
			RgvErrCache.updateRgvErr(slave.getId(),"未查到小车作业站"+staProtocol.getStaNo()+"站点");
			break;
			}
			// 判断是否满足入库条件
			if (!staProtocol.isAutoing() \|\| !staProtocol.isLoading() \|\| staProtocol.getWorkNo()==0
			){
			RgvErrCache.updateRgvErr(slave.getId(),"小车等待"+staProtocol.getStaNo()+"站点就绪");
			break;
			}
			}
			TaskProtocol issued = new TaskProtocol(taskProtocol);
			write(issued);
			RgvErrCache.updateRgvErr(slave.getId());
			taskProtocolCache.removeTaskProtocol(taskProtocol.getTaskNoDirection());
			break;
			} else {
			TaskProtocol issued = new TaskProtocol(taskProtocol,true);
			write(issued);
			RgvErrCache.updateRgvErr(slave.getId());
			break;
			}
			}
			}
			}
			rgvRun.currentTimeMilliOld = System.currentTimeMillis()+50;
			rgvRun.setRgvNo(slave.getOtherId());
			RgvRunCache.updateRgvStatus(rgvRun);
			} catch (Exception e) {
			log.error("RGV任务下发线程异常！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV任务下发线程异常"+e.getMessage());
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			rgvRun = RgvRunCache.getRgvRun();
			rgvRun.currentTimeMilliOld = System.currentTimeMillis()+50;
			rgvRun.setRgvNo(slave.getOtherId());
			RgvRunCache.updateRgvStatus(rgvRun);
			continue;
			// e.printStackTrace();
			}
			}
			}
			public boolean isSignRgvTaskProtocol() {
			RgvTaskProtocol rgvTaskProtocol = RgvTaskCache.getRgvStatus(slave.getId());
			if (rgvTaskProtocol == null) {
			return false;
			}
			RgvProtocol rgvProtocolOther = RgvStatusCache.getRgvStatus(slave.getOtherId());
			RgvTaskProtocol rgvTaskProtocolOther = RgvTaskCache.getRgvStatus(slave.getOtherId());

			if (rgvProtocolOther == null) {
			return false;
			}
			if (rgvProtocolOther.statusEnable) {
			if (rgvTaskProtocolOther == null) {
			return false;
			}
			}
			return true;
			}


			public boolean deviceDetection() {
			RgvProtocol rgvProtocol = RgvStatusCache.getRgvStatus(slave.getId());
			RgvTaskProtocol rgvTaskProtocol = RgvTaskCache.getRgvStatus(slave.getId());

			if (rgvProtocol == null \|\| rgvTaskProtocol == null) {
			errorRgv = slave.getId()+"号小车连接失败";
			return false;
			}
			if (!rgvProtocol.getModeType().equals(RgvModeType.AUTO)) {
			errorRgv = slave.getId()+"号小车非自动";
			return false;
			}
			if (rgvProtocol.getRgvPos().equals(0L)
			\|\| rgvTaskProtocol.getAvoid() == -1
			\|\| rgvProtocol.getRgvPosDestination() == 0L) {
			errorRgv = slave.getId()+"号小车状态异常";
			return false;
			}
			if (!rgvProtocol.getStatusType().equals(RgvStatusType.IDLE) && !rgvProtocol.getStatusType().equals(RgvStatusType.ROAM)) {
			errorRgv = "无";
			return false;
			}
			if ( (Math.abs(rgvProtocol.getRgvPos() - rgvProtocol.getRgvPosDestination())>100) && !rgvProtocol.getStatusType().equals(RgvStatusType.ROAM)) {
			errorRgv = slave.getId()+"号小车存在运行目标值，需要复位！！！";
			return false;
			}
			RgvProtocol rgvProtocolOther = RgvStatusCache.getRgvStatus(slave.getOtherId());
			RgvTaskProtocol rgvTaskProtocolOther = RgvTaskCache.getRgvStatus(slave.getOtherId());
			// System.out.println("rgvTaskProtocol:"+slave.getId()+"sign:"+rgvTaskProtocol.isSignRun()+"/n"+
			// "rgvTaskProtocolOther:"+slave.getOtherId()+"sign:"+rgvTaskProtocolOther.isSignRun()+"/n");

			if (rgvProtocolOther == null) {
			errorRgv = slave.getOtherId()+"号小车连接失败";
			return false;
			}
			if (rgvProtocolOther.statusEnable) {
			if (!rgvProtocolOther.getModeType().equals(RgvModeType.AUTO) \|\| rgvProtocolOther.getRgvPos().equals(0L) \|\| rgvTaskProtocolOther.getAvoid() == -1 \|\| rgvProtocolOther.getRgvPosDestination() == 0L) {
			errorRgv = slave.getOtherId()+"号小车状态异常";
			return false;
			}
			}
			return true;
			}

			public boolean rgvOtherStatusEnable() {
			RgvProtocol rgvProtocolOther = RgvStatusCache.getRgvStatus(slave.getOtherId());
			if (rgvProtocolOther == null) {
			return true;
			}
			if (rgvProtocolOther.statusEnable) {
			// if (!rgvProtocolOther.getModeType().equals(RgvModeType.AUTO) \|\| rgvProtocolOther.getRgvPos().equals(0L)) {
			return true;
			// }
			}
			return false;
			}

			public boolean otherRgvAvoid(Long targetPosition) {
			RgvProtocol rgvProtocol = RgvStatusCache.getRgvStatus(slave.getId());
			long onePos = Math.abs(targetPosition - rgvProtocol.getRgvPos());
			if (onePos<50){
			return true;
			}
			RgvTaskProtocol rgvTaskProtocol = RgvTaskCache.getRgvStatus(slave.getId());

			RgvProtocol rgvProtocolOther = RgvStatusCache.getRgvStatus(slave.getOtherId());
			RgvTaskProtocol rgvTaskProtocolOther = RgvTaskCache.getRgvStatus(slave.getOtherId());
			Long[][] avoidRange = new TrackRangeUtils().avoidRange(slave, trackEntireLength, trackBenchmark, avoidDistance);

			if (new TrackRangeUtils().IsItSmall(slave)) {
			if (rgvProtocolOther.getStatusType().equals(RgvStatusType.IDLE) && (Math.abs(rgvProtocolOther.getRgvPos() - rgvProtocolOther.getRgvPosDestination())<50)) {
			if ((rgvProtocolOther.getRgvPos() - rgvProtocolOther.getCarBodyJiaoMing())
			- (targetPosition + rgvProtocol.getCarBodyKunPeng())
			> avoidDistance - 50) {//无需避让
			return true;
			} else {

			RgvThread rgvThreadOther = (RgvThread) SlaveConnection.get(SlaveType.Rgv, slave.getOtherId());
			TaskProtocolCache taskProtocolCacheOther = rgvThreadOther.getTaskProtocolCache();
			TaskProtocol nowPosRunTask = taskProtocolCacheOther.getNowPosRunTask(rgvProtocolOther.getRgvPos());
			if (!Cools.isEmpty(nowPosRunTask)){
			boolean signNowPosRun = true;
			if (nowPosRunTask.getTaskStatus()==3){
			DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, nowPosRunTask.getTargetPositionStaNoPlcId());
			StaProtocol staProtocol = devpThread.getStation().get(nowPosRunTask.getTargetPositionStaNo());
			if (staProtocol == null ) {
			signNowPosRun =false;
			}
			if (signNowPosRun){
			// 判断是否满足入库条件
			if (!staProtocol.isAutoing() \|\| staProtocol.isLoading()
			){
			if (targetPosition.equals(nowPosRunTask.getTargetPosition())){
			if (rgvProtocolOther.getRgvPosDestination() >= rgvProtocolOther.getRgvPos()-50) {
			long avoid = rgvProtocolOther.getRgvPos() - rgvProtocolOther.getCarBodyJiaoMing() - avoidDistance - rgvProtocol.getCarBodyKunPeng();
			if (!new TrackRangeUtils().avoidRange(avoid, avoidRange[1])) {
			log.error("RGV行走超出范围！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			errorRgv = "RGV行走目标超出范围";
			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV行走超出范围");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			return false;
			}
			long avoidAbs = Math.abs(avoid - rgvProtocol.getRgvPos());
			if (avoidAbs<51){
			return false;
			}
			rgvTaskProtocol.setAvoid(1);
			rgvTaskProtocol.setAvoidingTheDestination(avoid);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocol);
			}
			return false;
			}
			signNowPosRun =false;
			}
			}
			}
			if (signNowPosRun){
			errorRgv = slave.getOtherId()+"号RGV等待作业...";
			return false;
			}
			}
			TaskProtocol nowPosRunTask2 = taskProtocolCacheOther.getNowPosRunTask2();
			if (!Cools.isEmpty(nowPosRunTask2)){
			boolean signNowPosRun = true;
			boolean signNowPosA = true;
			if (targetPosition - rgvProtocol.getRgvPos()>0){
			if (nowPosRunTask2.getTargetPosition() - rgvProtocolOther.getRgvPos()>0){
			if (rgvProtocol.getRgvPos()>rgvProtocolOther.getRgvPos()){
			signNowPosA = false;
			signNowPosRun = false;
			}
			}
			} else if (targetPosition - rgvProtocol.getRgvPos()<0){
			if (nowPosRunTask2.getTargetPosition() - rgvProtocolOther.getRgvPos()<0){
			if (rgvProtocol.getRgvPos()<rgvProtocolOther.getRgvPos()){
			signNowPosA = false;
			signNowPosRun = false;
			}
			}
			}

			if (signNowPosA){
			if (nowPosRunTask2.getTaskStatus()==3){
			long twoPos = Math.abs(nowPosRunTask2.getTargetPosition() - rgvProtocolOther.getRgvPos());
			if (Math.abs(onePos - twoPos)>100){
			if (onePos-50>twoPos+50){
			DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, nowPosRunTask2.getTargetPositionStaNoPlcId());
			StaProtocol staProtocol = devpThread.getStation().get(nowPosRunTask2.getTargetPositionStaNo());
			if (staProtocol == null ) {
			signNowPosRun =false;
			}
			if (signNowPosRun){
			// 判断是否满足入库条件
			if (staProtocol.isAutoing() && !staProtocol.isLoading()
			){
			errorRgv = slave.getOtherId()+"号RGV等待作业...";
			return false;
			}
			}
			signNowPosRun =false;
			}
			}
			} else {
			long twoPos = Math.abs(nowPosRunTask2.getTargetPosition() - rgvProtocolOther.getRgvPos());
			if (twoPos<100){
			errorRgv = slave.getOtherId()+"号RGV等待作业...";
			return false;
			}
			if (Math.abs(onePos - twoPos)>100){
			if (onePos-50>twoPos+50){
			signNowPosRun =false;
			}
			}
			}
			}

			if (signNowPosRun){
			errorRgv = slave.getOtherId()+"号RGV等待作业...";
			return false;
			}
			}
			long avoid = targetPosition + rgvProtocol.getCarBodyKunPeng() + avoidDistance + rgvProtocolOther.getCarBodyJiaoMing();
			if (!new TrackRangeUtils().avoidRange(avoid, avoidRange[0])) {
			log.error("RGV行走超出范围！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			errorRgv = "RGV行走目标超出范围";

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV行走超出范围");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			return false;
			}
			long avoidAbs = Math.abs(avoid - rgvProtocolOther.getRgvPos());
			if (avoidAbs<51){
			return true;
			}
			rgvTaskProtocolOther.setAvoid(1);
			rgvTaskProtocolOther.setAvoidingTheDestination(avoid);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocolOther);
			return false;
			}
			} else if (rgvProtocolOther.getStatusType().equals(RgvStatusType.ROAM)) {
			if ((rgvProtocolOther.getRgvPosDestination() - rgvProtocolOther.getCarBodyJiaoMing())
			- (targetPosition + rgvProtocol.getCarBodyKunPeng())
			> avoidDistance - 50) {//无需避让
			return true;
			} else {
			long avoid = targetPosition + rgvProtocol.getCarBodyKunPeng() + avoidDistance + rgvProtocolOther.getCarBodyJiaoMing();
			if (!new TrackRangeUtils().avoidRange(avoid, avoidRange[0])) {
			log.error("RGV行走超出范围！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			errorRgv = "RGV行走目标超出范围";

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV行走超出范围");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			return false;
			}
			long avoidAbs = Math.abs(avoid - rgvProtocolOther.getRgvPos());
			if (avoidAbs<51){
			return true;
			}
			rgvTaskProtocolOther.setAvoid(1);
			rgvTaskProtocolOther.setAvoidingTheDestination(avoid);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocolOther);
			return false;
			}
			} else if (!rgvProtocolOther.getStatusType().equals(RgvStatusType.IDLE)) {
			if ((rgvProtocolOther.getRgvPosDestination() - rgvProtocolOther.getCarBodyJiaoMing())
			- (targetPosition + rgvProtocol.getCarBodyKunPeng())
			> avoidDistance - 50) {//无需避让
			return true;
			} else {
			if (rgvProtocolOther.getRgvPosDestination() >= rgvProtocolOther.getRgvPos()-50) {
			long avoid = rgvProtocolOther.getRgvPos() - rgvProtocolOther.getCarBodyJiaoMing() - avoidDistance - rgvProtocol.getCarBodyKunPeng();
			if (!new TrackRangeUtils().avoidRange(avoid, avoidRange[1])) {
			log.error("RGV行走超出范围！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			errorRgv = "RGV行走目标超出范围";

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV行走超出范围");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			return false;
			}
			long avoidAbs = Math.abs(avoid - rgvProtocol.getRgvPos());
			if (avoidAbs<51){
			return false;
			}
			rgvTaskProtocol.setAvoid(1);
			rgvTaskProtocol.setAvoidingTheDestination(avoid);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocol);
			return false;
			}
			errorRgv = slave.getOtherId()+"号RGV影响，等待中...";

			return false;
			}
			} else {
			if ((rgvProtocolOther.getRgvPosDestinationOrPos(true) - rgvProtocolOther.getCarBodyJiaoMing())
			- (targetPosition + rgvProtocol.getCarBodyKunPeng())
			> avoidDistance - 50) {//无需避让
			return true;
			}
			}
			} else {
			if (rgvProtocolOther.getStatusType().equals(RgvStatusType.IDLE) && (Math.abs(rgvProtocolOther.getRgvPos() - rgvProtocolOther.getRgvPosDestination())<50)) {
			if ((targetPosition - rgvProtocol.getCarBodyJiaoMing())
			- (rgvProtocolOther.getRgvPos() + rgvProtocolOther.getCarBodyKunPeng())
			> avoidDistance - 50) {//无需避让
			return true;
			} else {
			RgvThread rgvThreadOther = (RgvThread) SlaveConnection.get(SlaveType.Rgv, slave.getOtherId());
			TaskProtocolCache taskProtocolCacheOther = rgvThreadOther.getTaskProtocolCache();
			TaskProtocol nowPosRunTask = taskProtocolCacheOther.getNowPosRunTask(rgvProtocolOther.getRgvPos());
			if (!Cools.isEmpty(nowPosRunTask)){
			boolean signNowPosRun = true;
			if (nowPosRunTask.getTaskStatus()==3){
			DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, nowPosRunTask.getTargetPositionStaNoPlcId());
			StaProtocol staProtocol = devpThread.getStation().get(nowPosRunTask.getTargetPositionStaNo());
			if (staProtocol == null ) {
			signNowPosRun =false;
			}
			if (signNowPosRun){
			// 判断是否满足入库条件
			if (!staProtocol.isAutoing() \|\| staProtocol.isLoading()
			){
			if (targetPosition.equals(nowPosRunTask.getTargetPosition())){
			if (rgvProtocolOther.getRgvPosDestination() <= rgvProtocolOther.getRgvPos() + 50) {
			long avoid = rgvProtocolOther.getRgvPos() + rgvProtocolOther.getCarBodyKunPeng() + avoidDistance + rgvProtocol.getCarBodyJiaoMing();
			if (!new TrackRangeUtils().avoidRange(avoid, avoidRange[0])) {
			log.error("RGV行走超出范围！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			errorRgv = "RGV行走目标超出范围";

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV行走超出范围");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			return false;
			}
			long avoidAbs = Math.abs(avoid - rgvProtocol.getRgvPos());
			if (avoidAbs<51){
			return false;
			}
			rgvTaskProtocol.setAvoid(1);
			rgvTaskProtocol.setAvoidingTheDestination(avoid);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocol);
			}
			return false;
			}
			signNowPosRun =false;
			}
			}
			}
			if (signNowPosRun){
			errorRgv = slave.getOtherId()+"号RGV等待作业...";
			return false;
			}
			}

			TaskProtocol nowPosRunTask2 = taskProtocolCacheOther.getNowPosRunTask2();
			if (!Cools.isEmpty(nowPosRunTask2)){
			boolean signNowPosRun = true;
			boolean signNowPosA = true;
			if (targetPosition - rgvProtocol.getRgvPos()>0){
			if (nowPosRunTask2.getTargetPosition() - rgvProtocolOther.getRgvPos()>0){
			if (rgvProtocol.getRgvPos()>rgvProtocolOther.getRgvPos()){
			signNowPosA = false;
			signNowPosRun = false;
			}
			}
			} else if (targetPosition - rgvProtocol.getRgvPos()<0){
			if (nowPosRunTask2.getTargetPosition() - rgvProtocolOther.getRgvPos()<0){
			if (rgvProtocol.getRgvPos()<rgvProtocolOther.getRgvPos()){
			signNowPosA = false;
			signNowPosRun = false;
			}
			}
			}

			if (signNowPosA){
			if (nowPosRunTask2.getTaskStatus()==3){
			long twoPos = Math.abs(nowPosRunTask2.getTargetPosition() - rgvProtocolOther.getRgvPos());
			if (Math.abs(onePos - twoPos)>100){
			if (onePos-50>twoPos+50){
			DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, nowPosRunTask2.getTargetPositionStaNoPlcId());
			StaProtocol staProtocol = devpThread.getStation().get(nowPosRunTask2.getTargetPositionStaNo());
			if (staProtocol == null ) {
			signNowPosRun =false;
			}
			if (signNowPosRun){
			// 判断是否满足入库条件
			if (staProtocol.isAutoing() && !staProtocol.isLoading()
			){
			errorRgv = slave.getOtherId()+"号RGV等待作业...";
			return false;
			}
			}
			signNowPosRun =false;
			}
			}
			} else {
			long twoPos = Math.abs(nowPosRunTask2.getTargetPosition() - rgvProtocolOther.getRgvPos());
			if (twoPos<100){
			errorRgv = slave.getOtherId()+"号RGV等待作业...";
			return false;
			}
			if (Math.abs(onePos - twoPos)>100){
			if (onePos-50>twoPos+50){
			signNowPosRun =false;
			}
			}
			}
			}

			if (signNowPosRun){
			errorRgv = slave.getOtherId()+"号RGV等待作业...";
			return false;
			}
			}
			long avoid = targetPosition - rgvProtocol.getCarBodyJiaoMing() - avoidDistance - rgvProtocolOther.getCarBodyKunPeng();
			if (!new TrackRangeUtils().avoidRange(avoid, avoidRange[1])) {
			log.error("RGV行走超出范围！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			errorRgv = "RGV行走目标超出范围";

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV行走超出范围");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			return false;
			}
			long avoidAbs = Math.abs(avoid - rgvProtocolOther.getRgvPos());
			if (avoidAbs<51){
			return true;
			}
			rgvTaskProtocolOther.setAvoid(1);
			rgvTaskProtocolOther.setAvoidingTheDestination(avoid);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocolOther);
			return false;
			}
			} else if (rgvProtocolOther.getStatusType().equals(RgvStatusType.ROAM)) {
			if ((targetPosition - rgvProtocol.getCarBodyJiaoMing())
			- (rgvProtocolOther.getRgvPosDestination() + rgvProtocolOther.getCarBodyKunPeng())
			> avoidDistance - 50) {//无需避让
			return true;
			} else {
			long avoid = targetPosition - rgvProtocol.getCarBodyJiaoMing() - avoidDistance - rgvProtocolOther.getCarBodyKunPeng();
			if (!new TrackRangeUtils().avoidRange(avoid, avoidRange[1])) {
			log.error("RGV行走超出范围！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			errorRgv = "RGV行走目标超出范围";

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV行走超出范围");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			return false;
			}
			long avoidAbs = Math.abs(avoid - rgvProtocolOther.getRgvPos());
			if (avoidAbs<51){
			return true;
			}
			rgvTaskProtocolOther.setAvoid(1);
			rgvTaskProtocolOther.setAvoidingTheDestination(avoid);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocolOther);
			return false;
			}

			} else if (!rgvProtocolOther.getStatusType().equals(RgvStatusType.IDLE)) {
			if ((targetPosition - rgvProtocol.getCarBodyJiaoMing())
			- (rgvProtocolOther.getRgvPosDestination() + rgvProtocolOther.getCarBodyKunPeng())
			> avoidDistance-50) {//无需避让
			return true;
			} else {
			if (rgvProtocolOther.getRgvPosDestination() <= rgvProtocolOther.getRgvPos() + 50) {
			long avoid = rgvProtocolOther.getRgvPos() + rgvProtocolOther.getCarBodyKunPeng() + avoidDistance + rgvProtocol.getCarBodyJiaoMing();
			if (!new TrackRangeUtils().avoidRange(avoid, avoidRange[0])) {
			log.error("RGV行走超出范围！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			errorRgv = "RGV行走目标超出范围";

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV行走超出范围");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			return false;
			}
			long avoidAbs = Math.abs(avoid - rgvProtocol.getRgvPos());
			if (avoidAbs<51){
			return false;
			}
			rgvTaskProtocol.setAvoid(1);
			rgvTaskProtocol.setAvoidingTheDestination(avoid);
			RgvTaskCache.updateRgvStatus(rgvTaskProtocol);
			return false;
			}
			errorRgv = slave.getOtherId()+"号RGV等待中...";
			return false;
			}
			} else {
			if ((rgvProtocolOther.getRgvPosDestinationOrPos(false) - rgvProtocolOther.getCarBodyJiaoMing())
			- (targetPosition + rgvProtocol.getCarBodyKunPeng())
			> avoidDistance - 50) {//无需避让
			return true;
			}
			}
			}

			errorRgv = slave.getOtherId()+"号RGV影响，等待中...";
			return false;
			}

			/**
			* 初始化RGV状态
			*/
			private void initRgv() {
			if (null == rgvProtocol) {
			RgvProtocol rgvProtocol = RgvStatusCache.getRgvStatus(slave.getId());
			if (rgvProtocol == null) {
			rgvProtocol = new RgvProtocol();
			rgvProtocol.setRgvNo(slave.getId());
			}
			//小车状态
			rgvProtocol.setStatus((short)-1);
			rgvProtocol.setRgvPosDestination(0);
			rgvProtocol.setMode((short) -1);
			rgvProtocol.setRgvPos(0);
			rgvProtocol.setErr1(false);
			rgvProtocol.setErr2(false);
			rgvProtocol.setErr3(false);
			rgvProtocol.setErr4(false);
			rgvProtocol.setErr5(false);
			rgvProtocol.setErr6(false);
			rgvProtocol.setErr7(false);
			rgvProtocol.setWrkTaskPri((short)0);
			//工位1状态
			rgvProtocol.setTaskNo1(0);
			rgvProtocol.setStatus1((short)-1);
			rgvProtocol.setLoaded1(false);
			rgvProtocol.setWrkTaskMove1((short)0);
			//工位2状态
			rgvProtocol.setTaskNo2(0);
			rgvProtocol.setStatus2((short)-1);
			rgvProtocol.setLoaded2(false);
			rgvProtocol.setWrkTaskMove2((short)0);
			// rgvProtocol.setAlarm((short)0);
			// rgvProtocol.setxSpeed((short) 0);
			// rgvProtocol.setxDistance((short) 0);
			// rgvProtocol.setxDuration((short) 0);
			}
			private ArrayList<Integer> getStaNo() {
			switch (slave.getId()) {
			case 1:
			return staNos1;
			case 2:
			return staNos2;
			default:
			throw new CoolException("服务器异常");
			rgvProtocol.setStatus((short) -1);
			rgvProtocol.setWalkPos((short) 0);
			rgvProtocol.setRgvPos(0L);
			rgvProtocol.setAlarm((short) 0);
			rgvProtocol.setxSpeed((short) 0);
			rgvProtocol.setxDistance((short) 0);
			rgvProtocol.setxDuration((short) 0);
			rgvProtocol.setCarBodyJiaoMing(0L);
			rgvProtocol.setCarBodyKunPeng(0L);
			try {
			BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class);
			BasRgv rgv = basRgvService.selectById(slave.getId());
			if (!Cools.isEmpty(rgv)) {
			rgvProtocol.setStatusEnable(rgv.getStatus() == 1);
			} else {
			rgvProtocol.setStatusEnable(false);
			}
			} catch (Exception e) {
			log.error("RGV异常！！！ ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV异常"+e.getMessage());
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			rgvProtocol.setStatusEnable(true);
			}

			RgvTaskProtocol rgvTaskProtocol = RgvTaskCache.getRgvStatus(slave.getId());
			if (rgvTaskProtocol == null) {
			rgvTaskProtocol = new RgvTaskProtocol();
			rgvTaskProtocol.setRgvNo(slave.getId());
			}
			rgvTaskProtocol.setAvoid(0);
			rgvTaskProtocol.setAvoidingTheDestination(0L);

			RgvStatusCache.updateRgvStatus(rgvProtocol);

			RgvTaskCache.updateRgvStatus(rgvTaskProtocol);

			}

			@Override
			public boolean connect() {
			boolean result = false;
			@@ -218,458 +1012,375 @@
			siemensNet.setRack(slave.getRack().byteValue());
			siemensNet.setSlot(slave.getSlot().byteValue());
			OperateResult connect = siemensNet.ConnectServer();
			if(connect.IsSuccess){
			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()));
			// 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()));
			// 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();

			if (System.currentTimeMillis()-currentTimeMilliConnectRgv>10006010){
			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGVplc连接失败");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			if (currentTimeMilliConnectRgv == 0){
			currentTimeMilliConnectRgv = System.currentTimeMillis()-10006010-1;
			} else {
			currentTimeMilliConnectRgv = System.currentTimeMillis();
			}
			}
			}
			initRgv();
			// siemensNet.ConnectClose();
			return result;
			}

			private void setBool(byte[] buffer, int byteIndex, int bitIndex, boolean value) {
			if (value) {
			buffer[byteIndex] \|= (1 << bitIndex); // 置位
			} else {
			buffer[byteIndex] &= ~(1 << bitIndex); // 清零
			}
			}
			private void updateFlagInDb(String field, boolean value) {
			try {
			BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class);

			EntityWrapper<BasRgv> wrapper = new EntityWrapper<>();
			wrapper.eq("rgv_no", slave.getId());

			BasRgv entity = new BasRgv();
			switch (field) {
			case "pak_mk": entity.setPakMk(value ? "1" : "0"); break;
			case "pak_in": entity.setPakIn(value ? "1" : "0"); break;
			case "pak_out": entity.setPakOut(value ? "1" : "0"); break;
			case "pak_rgv": entity.setPakRgv(value ? "1" : "0"); break;
			case "pak_to_crn":entity.setPakToCrn(value ? "1" : "0"); break;
			case "paking":entity.setPaking(value ? "1" : "0"); break;
			case "pak_all":entity.setPakAll(value ? "1" : "0"); break;
			default:
			log.warn("未知标志位字段: {}", field);
			return;
			}

			basRgvService.update(entity, wrapper);
			// log.info("RGV标志位更新成功 [rgv_no:{}] {}={}", slave.getId(), field, value ? "1" : "0");

			} catch (Exception e) {
			log.error("更新RGV标志位异常 [rgv_no:{}] {}={} 错误：{}", slave.getId(), field, value, e.getMessage());
			}
			}


			public void setPakMk(boolean pakMk) {
			this.PakMk = pakMk;
			updateFlagInDb("pak_mk", pakMk);
			}

			/**
			* 读取状态
			*/
			private void readStatus(){
			private void readStatus() {
			try {
			ArrayList<Integer> staNos = getStaNo();

			OperateResultExOne<byte[]> result = siemensNet.Read("DB101.0", (short) 38);
			OperateResultExOne<byte[]> result = siemensNet.Read("DB101.0", (short) 36);
			OperateResultExOne<byte[]> result100 = siemensNet.Read("DB100.0", (short) 18);
			if (result.IsSuccess) {
			if (null == rgvProtocol) {
			// 构建设备状态对象
			RgvProtocol rgvProtocol = RgvStatusCache.getRgvStatus(slave.getId());
			if (rgvProtocol == null) {
			rgvProtocol = new RgvProtocol();
			rgvProtocol.setRgvNo(siemensNet.getByteTransform().TransInt16(result.Content, 0));

			}
			rgvProtocol.setMode(siemensNet.getByteTransform().TransInt16(result.Content, 2));
			rgvProtocol.setRgvPos(siemensNet.getByteTransform().TransInt32(result.Content, 4));
			rgvProtocol.setRgvPosDestination( siemensNet.getByteTransform().TransInt32(result.Content, 8));
			rgvProtocol.setStatus1(siemensNet.getByteTransform().TransInt16(result.Content, 12));
			rgvProtocol.setTaskNo1(siemensNet.getByteTransform().TransInt32(result.Content, 16));
			rgvProtocol.setTaskNo2(siemensNet.getByteTransform().TransInt32(result.Content, 20));
			rgvProtocol.setStaNo1(siemensNet.getByteTransform().TransInt32(result.Content, 28));
			rgvProtocol.setStaNo2(siemensNet.getByteTransform().TransInt32(result.Content, 32));
			boolean[] status1 = siemensNet.getByteTransform().TransBool(result.Content, 24, 1);
			rgvProtocol.setLoaded1(status1[0]);
			rgvProtocol.setLoaded2(status1[1]);
			boolean[] status2 = siemensNet.getByteTransform().TransBool(result.Content, 36,1);
			rgvProtocol.setErr1(status2[0]);
			rgvProtocol.setErr2(status2[1]);
			rgvProtocol.setErr3(status2[2]);
			rgvProtocol.setErr4(status2[3]);
			rgvProtocol.setErr5(status2[4]);
			rgvProtocol.setErr6(status2[5]);
			rgvProtocol.setErr7(status2[6]);
			rgvProtocol.setErr8(status2[7]);
			boolean[] status3 = siemensNet.getByteTransform().TransBool(result.Content, 37, 1);
			rgvProtocol.setErr9(status3[0]);
			rgvProtocol.setErr10(status3[1]);
			rgvProtocol.setErr11(status3[2]);
			rgvProtocol.setErr12(status3[3]);
			// BasRgvMapService basRgvMapService = SpringUtils.getBean(BasRgvMapService.class);
			rgvProtocol.setRgvNo(slave.getId());
			rgvProtocol.setCarBodyJiaoMing(slave.getCarBodyJiaoMing());
			rgvProtocol.setCarBodyKunPeng(slave.getCarBodyKunPeng());

			rgvProtocol.setMode(siemensNet.getByteTransform().TransInt16(result.Content, 0));
			// rgvProtocol.setStatus(siemensNet.getByteTransform().TransInt16(result.Content, 2));
			rgvProtocol.setTaskNo((long)siemensNet.getByteTransform().TransInt32(result.Content, 4));
			rgvProtocol.setStatus(siemensNet.getByteTransform().TransInt16(result.Content, 8));
			rgvProtocol.setLoaded(siemensNet.getByteTransform().TransInt16(result.Content, 10));
			rgvProtocol.setRgvPos((long)siemensNet.getByteTransform().TransInt32(result.Content, 32));
			rgvProtocol.setRgvPosDestination((long)siemensNet.getByteTransform().TransInt32(result100.Content, 14));
			if (rgvProtocol.getRgvPosDestination()==0L){
			rgvProtocol.setRgvPosDestination(rgvProtocol.getRgvPos());
			}

			//作业启动中解决任务下发后小车状态未更新，小车状态不及时
			if (this.wrkSign == 1 && rgvProtocol.getStatus()!=(short)1){
			this.wrkSign = 0;
			}
			if (this.wrkSign == 1){
			rgvProtocol.setStatus((short)110);
			}

			// rgvProtocol.setAlarm(siemensNet.getByteTransform().TransInt16(result.Content, 20));
			// rgvProtocol.setxSpeed(siemensNet.getByteTransform().TransInt16(result.Content, 28));
			// rgvProtocol.setxDistance(siemensNet.getByteTransform().TransInt16(result.Content, 40));
			rgvProtocol.setAlarm(siemensNet.getByteTransform().TransInt16(result.Content, 16));

			// OperateResultExOne<byte[]> result11 = siemensNet.Read("DB100.12", (short) 1);
			// boolean[] status = siemensNet.getByteTransform().TransBool(result.Content, 18, 2);
			// rgvProtocol.setLoaded(status[0]? (short)1:(short)0);
			// rgvProtocol.setErr1(status[8]);
			// rgvProtocol.setErr2(status[9]);
			// rgvProtocol.setErr3(status[10]);
			// rgvProtocol.setErr4(status[11]);
			// rgvProtocol.setErr5(status[12]);
			// rgvProtocol.setErr6(status[13]);
			// rgvProtocol.setErr7(status[14]);
			// rgvProtocol.setErr8(status[15]);
			// System.out.println("读线程"+ slave.getId()+"---"+JSON.toJSONString(rgvProtocol));
			// System.out.println("读线程,小车号"+ slave.getId()+"时间戳："+System.currentTimeMillis()+"小车数据："+JSON.toJSONString(rgvProtocol));
			OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] <<<<< 实时数据更新成功", DateUtils.convert(new Date()), slave.getId()));
			// // 工位1复位信号
			// if (rgvProtocol.getStatusType().equals(RgvStatusType.WAITING)
			// \|\| rgvProtocol.getStatusType().equals(RgvStatusType.FETCHWAITING)) {
			// if (resetFlag1) {
			// RgvCommand rgvCommand = new RgvCommand();
			// rgvCommand.setAckFinish1((short)1);
			// if (write(rgvCommand)) {
			// resetFlag1 = false;
			// }
			// }
			// }
			// if (rgvProtocol.getAlarm$()!=0){
			// RgvErrCache.updateRgvErr(slave.getId(),rgvProtocol.getAlarmM());
			// }

			try {
			// 根据实时信息更新数据库
			BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class);
			BasRgv basRgv = new BasRgv();
			basRgv.setRgvNo(slave.getId());
			basRgv.setRgvSts((int)rgvProtocol.getMode());
			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());
			BasRgv basRgv = basRgvService.selectById(slave.getId());
			if (!Cools.isEmpty(basRgv)) {
			rgvProtocol.setStatusEnable(basRgv.getStatus() == 1);
			} else {
			rgvProtocol.setStatusEnable(false);
			}
			} catch (Exception ignore){
			System.out.println(ignore);
			// BasRgv basRgv = new BasRgv();
			basRgv.setRgvNo(slave.getId());
			basRgv.setRgvSts((int) rgvProtocol.getMode());
			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{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV plc数据库更新失败");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			}
			} catch (Exception ignore) {

			}


			// 更新缓存
			RgvStatusCache.updateRgvStatus(rgvProtocol);

			} 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());
			connectRgv = false;
			// 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());

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "读取RGV plc状态信息失败");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			}
			} 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());
			// 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());

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "读取RGV plc状态信息失败");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			initRgv();
			}
			}

			/**
			* 小车工位写入写入数据
			* 写入数据
			*/
			private boolean write(RgvCommand command) throws InterruptedException {
			if (null == command) {
			log.error("RGV写入命令为空");
			return false;
			}
			OperateResultExOne<byte[]> result1 = siemensNet.Read("DB100.0", (short) 39);
			if (result1.IsSuccess){
			RgvCommand one = new RgvCommand();
			one.setWrkTaskPri(siemensNet.getByteTransform().TransInt32(result1.Content, 0));//执行工位
			one.setTargetPosition(siemensNet.getByteTransform().TransInt32(result1.Content, 4));//行走目标站
			one.setTaskStatus(siemensNet.getByteTransform().TransInt16(result1.Content, 8));//小车状态
			one.setTaskNo(siemensNet.getByteTransform().TransInt32(result1.Content, 10));//下发工作号
			News.info("RGV命令下发前读取状态[id:{}] >>>>> 写入[{}],===>>回读[{}]", slave.getId(), JSON.toJSON(command),JSON.toJSON(one));
			}
			byte[] writeBytes = new byte[14];
			command.setRgvNo(slave.getId());
			if(specialSites.contains(command.getTargetPosition())){
			command.setTargetPosition(StaPosition.get(command.getTargetPosition()));
			}
			writeInt32(writeBytes, 0, command.getWrkTaskPri());
			writeInt32(writeBytes, 4, command.getTargetPosition());
			writeInt16(writeBytes, 8, (short)command.getTaskStatus());
			writeInt32(writeBytes, 10, command.getTaskNo());
			OperateResult result = siemensNet.Write("DB100.0", writeBytes);
			private boolean write(TaskProtocol taskProtocol) throws InterruptedException {
			Thread.sleep(50L);
			if (null == taskProtocol) {
			log.error("RGV写入命令为空 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());

			if (!result.IsSuccess){
			News.error("写入RGVplc数据失败，重新添加任务到队列 ===> [id:{}],{}",slave.getId(),JSON.toJSON(command));
			MessageQueue.offer(SlaveType.Rgv,slave.getId(),new Task(2,command));
			Thread.sleep(100);
			readStatus();
			return false;
			}

			//RGV任务写入后，回读一次，看是否成功
			Thread.sleep(400);
			try {
			OperateResultExOne<byte[]> resultRead = siemensNet.Read("DB100.0", (short) 14);
			if (resultRead.IsSuccess){
			RgvCommand one = new RgvCommand();
			one.setWrkTaskPri(siemensNet.getByteTransform().TransInt32(resultRead.Content, 0));//执行工位
			one.setTargetPosition(siemensNet.getByteTransform().TransInt32(resultRead.Content, 4));//行走目标站
			one.setTaskStatus(siemensNet.getByteTransform().TransInt16(resultRead.Content, 8));//小车状态
			one.setTaskNo(siemensNet.getByteTransform().TransInt32(resultRead.Content, 10));//下发工作号
			if (
			!command.getTaskNo().equals(one.getTaskNo()) \|\|
			!command.getTaskStatus().equals(one.getTaskStatus()) \|\|
			!command.getWrkTaskPri().equals(one.getWrkTaskPri()) \|\|
			!command.getTargetPosition().equals(one.getTargetPosition())
			) {
			try{
			News.error("RGV命令地址写入后回读失败[id:{}] >>>>> 写入[{}],===>>回读[{}]", slave.getId(), JSON.toJSON(command),JSON.toJSON(one));
			}catch (Exception e){
			try{
			News.error("日志打印失败：===>>参数one报错 [id:{}],{}", slave.getId(), JSON.toJSON(command),JSON.toJSON(resultRead));
			}catch (Exception e1){
			News.error("日志打印失败：===>> [id:{}],{}", slave.getId(), JSON.toJSON(command));
			}
			}
			News.error("Rgv命令回读失败后，重新添加任务到队列 ===>> [id:{}],{}", slave.getId(), JSON.toJSON(command));
			MessageQueue.offer(SlaveType.Rgv, slave.getId(), new Task(2, command));
			Thread.sleep(100);
			readStatus();
			return false;
			}else {
			News.info("RGV命令地址写入后回读成功[id:{}] >>>>> 写入[{}],===>>回读[{}]", slave.getId(), JSON.toJSON(command),JSON.toJSON(one));
			}
			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvErr", slave.getId(), "RGV写入命令为空");
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			}catch (Exception e){
			News.error("RGV命令地址写入后回读出错");
			return false;
			}

			if (result.IsSuccess) { //任务下发确认
			Thread.sleep(300);
			//任务下发次数
			int writeCount2 = 0;
			do {
			writeCount2++;
			Short commandFinish = 1;
			result = siemensNet.Write("DB100.34.0", commandFinish);
			if(result.IsSuccess){
			//RGV任务写入后，回读一次，看是否成功
			Thread.sleep(200);
			OperateResultExOne<byte[]> resultRead = siemensNet.Read("DB100.34", (short) 2);
			if (resultRead.IsSuccess) {
			commandFinish=siemensNet.getByteTransform().TransInt16(resultRead.Content, 0);
			if (commandFinish !=1){
			News.error("RGV任务确认位"+commandFinish+"写入数据与回读数据不一致！"+"循环执行次数:"+writeCount2+"次");
			}else{
			//任务命令写入成功
			News.info("RGV任务确认位"+commandFinish+"回读成功！"+"循环执行次数:"+writeCount2+"次");
			break;
			}
			}else {
			News.error("RGV任务确认位"+commandFinish+"回读失败！"+"循环执行次数:"+writeCount2+"次");
			}
			} else {
			News.error("RGV任务确认位"+commandFinish+"写入失败！"+"循环执行次数:"+writeCount2+"次");
			}
			}while (writeCount2<5);

			this.wrkSign = 1;
			// convertRow(command);
			// taskProtocol.setRgvNo(slave.getId());
			// Long[] array = new Long[11];
			OperateResult result0 = siemensNet.Write("DB100.0", (short) 0);
			// array[0] = taskProtocol.getAckFinish1();
			// array[1] = taskProtocol.getTaskNo();
			// array[2] = taskProtocol.getTaskMode();
			// array[4] = command.getDestinationStaNo();
			// array[10] = taskProtocol.getCommand();
			int taskStatus = taskProtocol.getTaskStatus$();
			int targetPositionStaNo = taskProtocol.getTargetPositionStaNo();
			int targetPositionStaNoEnd = taskProtocol.getTargetPositionStaNoEnd();
			boolean sign = true;
			int i = 1;
			while (sign && i<5){
			i++;
			sign = false;
			OperateResult result2 = siemensNet.Write("DB100.2", taskProtocol.getTaskNo().intValue());
			OperateResult result6 = siemensNet.Write("DB100.6", (short) taskStatus);
			OperateResult result8 = siemensNet.Write("DB100.8", (short) targetPositionStaNo);
			OperateResult result10 = siemensNet.Write("DB100.10", (short) targetPositionStaNo);
			OperateResult result14 = siemensNet.Write("DB100.14", taskProtocol.getTargetPosition().intValue());
			OperateResult result18 = siemensNet.Write("DB100.18", (short) targetPositionStaNoEnd);
			// OperateResult result3 = siemensNet.Write("DB100.10", taskProtocol.isDirection()? (short) 1 : (short) 2); // 执行方向（面朝轨道定位值左小右大） true:左 false:右
			Thread.sleep(10L);

			OperateResultExOne<byte[]> resultRead0 = siemensNet.Read("DB100.0", (short) 20);
			short i0 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 0);
			Integer i2 = siemensNet.getByteTransform().TransInt32(resultRead0.Content, 2);
			short i6 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 6);
			short i8 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 8);
			short i10 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 10);
			Integer i14 = siemensNet.getByteTransform().TransInt32(resultRead0.Content, 14);
			short i18 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 18);
			if (i0 !=(short) 0 \|\| i2!=taskProtocol.getTaskNo().intValue() \|\| i6 != (short) taskStatus
			\|\| i8!=(short)targetPositionStaNo \|\| i10 != (short) targetPositionStaNo
			\|\| i14!=taskProtocol.getTargetPosition().intValue() \|\| i18 != (short) targetPositionStaNoEnd){
			sign = true;
			}
			}
			sign = true;
			OperateResult result12 = siemensNet.Write("DB100.12", (short) 1);
			//
			i = 1;
			sign = true;
			while (sign && i<5){
			i++;
			sign = false;
			// OperateResult result3 = siemensNet.Write("DB100.10", taskProtocol.isDirection()? (short) 1 : (short) 2); // 执行方向（面朝轨道定位值左小右大） true:左 false:右
			Thread.sleep(10L);

			OperateResultExOne<byte[]> resultRead0 = siemensNet.Read("DB100.0", (short) 20);
			short i12 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 12);
			if (i12 != (short) 1 ){
			sign = true;
			}
			result12 = siemensNet.Write("DB100.12", (short) 1);
			}

			// log.info("任务下发线程,小车号"+ slave.getId()+"时间戳："+System.currentTimeMillis());

			// if (taskProtocol.getAckFinish1() == 0) {
			// short commandFinish = 3; //工位1、2任务同时写入
			// Thread.sleep(100L);
			// result = siemensNet.Write("DB100.20", commandFinish);
			// }

			try {
			// 日志记录
			BasRgvOptService bean = SpringUtils.getBean(BasRgvOptService.class);
			BasRgvOpt basRgvOpt = new BasRgvOpt(
			command.getTaskNo(),
			command.getRgvNo(),
			taskProtocol.getTaskNo().intValue(),
			taskProtocol.getTaskNo().intValue(),
			slave.getId(),
			new Date(),
			command.getTaskStatus().toString(),
			command.getTargetPosition(),
			command.getWrkTaskPri(),
			new Date()
			String.valueOf(taskProtocol.getTaskStatus()),
			null,
			null,
			null,
			result12.IsSuccess ? 1 : 0,
			null,
			new Date(),
			null
			);
			bean.insert(basRgvOpt);
			} catch (Exception ignore) {
			log.error(ignore.getMessage());
			}

			if (result != null && result.IsSuccess) {
			Thread.sleep(200);
			this.readStatus();
			log.info("RGV 工位命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSON(command));
			OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 工位命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSON(command)));
			readStatus();
			if (result12 != null && result0.IsSuccess) {
			// Thread.sleep(50);
			// this.readStatus();
			log.info("RGV 命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSON(taskProtocol));
			log.error("RGV 命令下发 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());

			try{
			DeviceErrorService deviceErrorService = SpringUtils.getBean(DeviceErrorService.class);
			deviceErrorService.addDeviceError("rgvWrite", slave.getId(), "RGV 命令下发"+JSON.toJSON(taskProtocol));
			} catch (Exception e2){
			// log.error("e2:"+e2.getMessage());
			}
			OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSON(taskProtocol)));
			return true;
			} else {
			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());
			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());
			return false;
			}
			}
			private boolean write2(RgvCommand command) throws InterruptedException {
			if (null == command) {
			log.error("RGV写入命令为空");
			return false;
			}
			OperateResultExOne<byte[]> result1 = siemensNet.Read("DB100.0", (short) 39);
			if (result1.IsSuccess){
			RgvCommand one = new RgvCommand();
			one.setWrkTaskPri(siemensNet.getByteTransform().TransInt32(result1.Content, 0));//执行工位
			one.setTargetPosition(siemensNet.getByteTransform().TransInt32(result1.Content, 4));//行走目标站
			one.setTaskStatus(siemensNet.getByteTransform().TransInt16(result1.Content, 8));//小车状态
			one.setTaskNo(siemensNet.getByteTransform().TransInt32(result1.Content, 10));//下发工作号
			News.info("RGV命令下发前读取状态[id:{}] >>>>> 写入[{}],===>>回读[{}]", slave.getId(), JSON.toJSON(command),JSON.toJSON(one));
			}
			byte[] writeBytes = new byte[36];
			command.setRgvNo(slave.getId());
			if(specialSites.contains(command.getTargetPosition())){
			command.setTargetPosition(StaPosition.get(command.getTargetPosition()));
			}
			writeInt32(writeBytes, 0, command.getWrkTaskPri());
			writeInt32(writeBytes, 4, command.getTargetPosition());
			writeInt16(writeBytes, 8, (short)command.getTaskStatus());
			writeInt32(writeBytes, 10, command.getTaskNo());
			writeBool(writeBytes, 34, 0, command.getCommand());
			OperateResult result = siemensNet.Write("DB100.0", writeBytes);

			if (!result.IsSuccess){
			News.error("写入RGVplc数据失败，重新添加任务到队列 ===> [id:{}],{}",slave.getId(),JSON.toJSON(command));
			MessageQueue.offer(SlaveType.Rgv,slave.getId(),new Task(2,command));
			Thread.sleep(100);
			readStatus();
			return false;
			}
			private boolean writeDelRgvTask() throws InterruptedException {
			//
			int i = 1;
			boolean sign = true;
			while (sign && i<5){
			i++;
			sign = false;
			OperateResult result0 = siemensNet.Write("DB100.0", (short) 0);
			OperateResult result2 = siemensNet.Write("DB100.2", 0);
			OperateResult result6 = siemensNet.Write("DB100.6", (short) 0);
			OperateResult result8 = siemensNet.Write("DB100.8", (short) 0);
			OperateResult result10 = siemensNet.Write("DB100.10", (short) 0);
			OperateResult result12 = siemensNet.Write("DB100.12",(short)0);
			OperateResult result14 = siemensNet.Write("DB100.14",0);
			OperateResult result18 = siemensNet.Write("DB100.18", (short) 0);
			// OperateResult result3 = siemensNet.Write("DB100.10", taskProtocol.isDirection()? (short) 1 : (short) 2); // 执行方向（面朝轨道定位值左小右大） true:左 false:右
			Thread.sleep(10L);

			//RGV任务写入后，回读一次，看是否成功
			Thread.sleep(400);
			try {
			OperateResultExOne<byte[]> resultRead = siemensNet.Read("DB100.0", (short) 14);
			if (resultRead.IsSuccess){
			RgvCommand one = new RgvCommand();
			one.setWrkTaskPri(siemensNet.getByteTransform().TransInt32(resultRead.Content, 0));//执行工位
			one.setTargetPosition(siemensNet.getByteTransform().TransInt32(resultRead.Content, 4));//行走目标站
			one.setTaskStatus(siemensNet.getByteTransform().TransInt16(resultRead.Content, 8));//小车状态
			one.setTaskNo(siemensNet.getByteTransform().TransInt32(resultRead.Content, 10));//下发工作号
			if (
			!command.getTaskNo().equals(one.getTaskNo()) \|\|
			!command.getTaskStatus().equals(one.getTaskStatus()) \|\|
			!command.getWrkTaskPri().equals(one.getWrkTaskPri()) \|\|
			!command.getTargetPosition().equals(one.getTargetPosition())
			) {
			try{
			News.error("RGV命令地址写入后回读失败[id:{}] >>>>> 写入[{}],===>>回读[{}]", slave.getId(), JSON.toJSON(command),JSON.toJSON(one));
			}catch (Exception e){
			try{
			News.error("日志打印失败：===>>参数one报错 [id:{}],{}", slave.getId(), JSON.toJSON(command),JSON.toJSON(resultRead));
			}catch (Exception e1){
			News.error("日志打印失败：===>> [id:{}],{}", slave.getId(), JSON.toJSON(command));
			}
			}
			News.error("Rgv命令回读失败后，重新添加任务到队列 ===>> [id:{}],{}", slave.getId(), JSON.toJSON(command));
			MessageQueue.offer(SlaveType.Rgv, slave.getId(), new Task(2, command));
			Thread.sleep(100);
			readStatus();
			return false;
			}else {
			News.info("RGV命令地址写入后回读成功[id:{}] >>>>> 写入[{}],===>>回读[{}]", slave.getId(), JSON.toJSON(command),JSON.toJSON(one));
			}
			OperateResultExOne<byte[]> resultRead0 = siemensNet.Read("DB100.0", (short) 20);
			short i0 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 0);
			Integer i2 = siemensNet.getByteTransform().TransInt32(resultRead0.Content, 2);
			short i6 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 6);
			short i8 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 8);
			short i10 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 10);
			short i12 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 12);
			Integer i14 = siemensNet.getByteTransform().TransInt32(resultRead0.Content, 14);
			short i18 = siemensNet.getByteTransform().TransInt16(resultRead0.Content, 18);
			if (i0 !=(short) 0 \|\| i2!=0 \|\| i6 != (short) 0
			\|\| i8!=(short)0 \|\| i10 != (short) 0 \|\| i12 != (short) 0
			\|\| i14!=0 \|\| i18 != (short) 0){
			sign = true;
			}
			}catch (Exception e){
			News.error("RGV命令地址写入后回读出错");
			}
			log.info("任务完成下发线程,小车号"+ slave.getId()+"时间戳："+System.currentTimeMillis());

			if (result.IsSuccess) { //任务下发确认
			Thread.sleep(300);
			//任务下发次数
			int writeCount2 = 0;
			do {
			writeCount2++;
			Short commandFinish = 1;
			result = siemensNet.Write("DB100.34.0", commandFinish);
			if(result.IsSuccess){
			//RGV任务写入后，回读一次，看是否成功
			Thread.sleep(200);
			OperateResultExOne<byte[]> resultRead = siemensNet.Read("DB100.34", (short) 2);
			if (resultRead.IsSuccess) {
			commandFinish=siemensNet.getByteTransform().TransInt16(resultRead.Content, 0);
			if (commandFinish !=1){
			News.error("RGV任务确认位"+commandFinish+"写入数据与回读数据不一致！"+"循环执行次数:"+writeCount2+"次");
			}else{
			//任务命令写入成功
			News.info("RGV任务确认位"+commandFinish+"回读成功！"+"循环执行次数:"+writeCount2+"次");
			break;
			}
			}else {
			News.error("RGV任务确认位"+commandFinish+"回读失败！"+"循环执行次数:"+writeCount2+"次");
			}
			} else {
			News.error("RGV任务确认位"+commandFinish+"写入失败！"+"循环执行次数:"+writeCount2+"次");
			}
			}while (writeCount2<5);
			}
			try {
			// 日志记录
			BasRgvOptService bean = SpringUtils.getBean(BasRgvOptService.class);
			BasRgvOpt basRgvOpt = new BasRgvOpt(
			command.getTaskNo(),
			command.getRgvNo(),
			0,
			0,
			slave.getId(),
			new Date(),
			command.getTaskStatus().toString(),
			command.getTargetPosition(),
			command.getWrkTaskPri(),
			new Date()
			String.valueOf(0),
			null,
			null,
			null,
			sign ? 1 : 0,
			null,
			new Date(),
			null
			);
			bean.insert(basRgvOpt);
			} catch (Exception ignore) {
			log.error(ignore.getMessage());
			}

			if (result != null && result.IsSuccess) {
			Thread.sleep(200);
			this.readStatus();
			log.info("RGV 工位命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSON(command));
			OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 工位命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSON(command)));
			if (sign) {
			// Thread.sleep(200);
			// this.readStatus();
			log.info("RGV 命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSON("null"));
			OutputQueue.RGV.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSON("null")));
			return true;
			} else {
			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());
			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());
			return false;
			}
			}
			public static void writeInt32(byte[] buffer, int index, int value) {
			buffer[index] = (byte)((value >> 24) & 0xFF);
			buffer[index + 1] = (byte)((value >> 16) & 0xFF);
			buffer[index + 2] = (byte)((value >> 8) & 0xFF);
			buffer[index + 3] = (byte)(value & 0xFF);


			/**
			* 清除作业启动中
			*/
			@Override
			public void setWrkSign() {
			this.wrkSign = 0;
			}


			public static void writeInt16(byte[] buffer, int index, short value) {
			buffer[index] = (byte)((value >> 8) & 0xFF); // 高字节
			buffer[index + 1] = (byte)(value & 0xFF); // 低字节
			}

			public static void writeBool(byte[] buffer, int byteIndex, int bitIndex, boolean value) {
			if (value) {
			buffer[byteIndex] \|= (1 << bitIndex);
			} else {
			buffer[byteIndex] &= ~(1 << bitIndex);
			}
			}

			public Integer getRgvPosI(Integer pos) {
			if (pos == null) return 0;

			// key: 站点号 value: 基准物理位置
			Map<Integer, Integer> posMap = new HashMap<>();
			posMap.put(1004, 6534);
			posMap.put(1007, 33634);
			posMap.put(1010, 75174);
			posMap.put(1014, 102124);
			posMap.put(1018, 138224);
			posMap.put(1021, 178034);
			posMap.put(1024, 219684);
			posMap.put(1028, 246724);
			posMap.put(1031, 288194);
			posMap.put(1035, 315204);
			int tolerance = 50; // 允许误差范围

			for (Map.Entry<Integer, Integer> entry : posMap.entrySet()) {
			int site = entry.getKey();
			int basePos = entry.getValue();
			if (Math.abs(pos - basePos) <= tolerance) {
			return site;
			}
			}

			return 0; // 没匹配到站点
			public void setDelRgvTask() {
			delRgvTask = true;
			}

			@Override
			public void close() {
			siemensNet.ConnectClose();
			}

			/******************************************************************************************/
			/************************************** 测试专用 ***************************************/
			/*****************************************************************************************/
			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);

			}

			}