diff --git a/opentcs-common/src/main/java/org/opentcs/kc/common/byteutils/ByteUtils.java b/opentcs-common/src/main/java/org/opentcs/kc/common/byteutils/ByteUtils.java index 43ad0ea..f26c563 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/common/byteutils/ByteUtils.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/common/byteutils/ByteUtils.java @@ -26,6 +26,13 @@ public class ByteUtils { System.arraycopy(src, start, dest, 0, length); return dest; } + public static byte[] copyBytesContainStart(byte[] src, int start, int length) { + byte[] dest = new byte[length]; + for (int i = 0; i < length; i++) { + dest[i] = src[i+start]; + } + return dest; + } /** * 将 float 类型的数值按照小端模式转换为字节数组 * @param value 要转换的 float 值 diff --git a/opentcs-common/src/main/java/org/opentcs/kc/common/enmuc/ModbusFC.java b/opentcs-common/src/main/java/org/opentcs/kc/common/enmuc/ModbusFC.java deleted file mode 100644 index b7a5072..0000000 --- a/opentcs-common/src/main/java/org/opentcs/kc/common/enmuc/ModbusFC.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.opentcs.kc.common.enmuc; - -public enum ModbusFC { - //心跳变量(这个可以要求电控同事加一个,不和业务关联,只用于通讯) - MB_HOLD_REG((byte) 0x03, (byte) 0x10), - Q_OUT((byte) 0x01, (byte) 0x05), - I_IN((byte) 0x02, null), - IW_IN((byte) 0x04, null), - ; - - private Byte fread; - private Byte fwrite; - - ModbusFC(Byte fread, Byte fwrite){ - this.fread = fread; - this.fwrite = fwrite; - } - - public Byte getFread() { - return fread; - } - - public void setFread(Byte fread) { - this.fread = fread; - } - - public Byte getFwrite() { - return fwrite; - } - - public void setFwrite(Byte fwrite) { - this.fwrite = fwrite; - } -} diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java index 064b326..3e801ea 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java @@ -7,8 +7,10 @@ import java.util.List; import org.opentcs.kc.common.byteutils.ByteUtils; import org.opentcs.kc.udp.agv.param.AgvEvent; import org.opentcs.kc.udp.agv.param.AgvEventConstant; +import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp; import org.opentcs.kc.udp.agv.param.function.b1.SubscribeInfo; import org.opentcs.kc.udp.agv.param.function.b1.SubscribeParam; +import org.opentcs.kc.udp.agv.param.function.b1.SubscribeRsp; import org.opentcs.kc.udp.agv.param.function.navigation.*; import org.opentcs.kc.udp.agv.param.function.read.ReadParam; import org.opentcs.kc.udp.agv.param.function.read.ReadStrValue; @@ -17,6 +19,7 @@ import org.opentcs.kc.udp.agv.param.function.write.WriteParam; import org.opentcs.kc.udp.agv.param.function.write.WriteStrValue; import org.opentcs.kc.udp.agv.param.function.write.WriteValueMember; import org.opentcs.kc.udp.agv.param.function.x14.RobotSetPosition; +import org.opentcs.kc.udp.agv.param.function.x17.QueryRobotRunStatusRsp; import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; import org.opentcs.kc.udp.io.UDPClient; import org.opentcs.kc.udp.agv.param.AgvEvent; @@ -24,12 +27,12 @@ import org.opentcs.kc.udp.agv.param.AgvEvent; /** * * AGV启动: - * 0xAF(查询机器人状态) 👌 + * 0xAF(查询机器人状态) 👌 有response * 0xB1(下发订阅信令) 👌 * 初始化: * 0x03(切换手自动) 👌 * 0x14(手动定位) 👌 - * 0x17(查询机器人运行状态) 👌 + * 0x17(查询机器人运行状态) 👌 有response * 0x1F(确认初始位置) 👌 * 运行: * 0xAE(导航控制导航点控制) 👌 @@ -41,23 +44,23 @@ import org.opentcs.kc.udp.agv.param.AgvEvent; */ public class KCCommandDemo { public static void main(String [] args) throws Exception{ -// { -// //0xAF(查询机器人状态) -// AgvEvent agvEvent = queryStatus(); -// printInfo(agvEvent); -// RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); -// if(rcv.isOk()){ -// QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes()); -// System.out.println(); -// System.out.println("received transationId : "+ "isok:"+rcv.isOk()); -// for (byte b:rcv.getValue()){ -// System.out.print(byteToHex(b)+" "); -// } -// }else { -// System.out.println(); -// System.out.println("received transationId : "+ "isok:"+rcv.isOk()); -// } -// } + { + //0xAF(查询机器人状态) + AgvEvent agvEvent = queryStatus(); + printInfo(agvEvent); + RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); + if(rcv.isOk()){ + QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes()); + System.out.println(); + System.out.println("received transationId : "+ "isok:"+rcv.isOk()); + for (byte b:rcv.getValue()){ + System.out.print(byteToHex(b)+" "); + } + }else { + System.out.println(); + System.out.println("received transationId : "+ "isok:"+rcv.isOk()); + } + } // { // //0xB0(查询载货状态) @@ -86,12 +89,12 @@ public class KCCommandDemo { // if(rcv.isOk()){ // System.out.println(); // System.out.println("received transationId : "+ "isok:"+rcv.isOk()); -// SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes()); -// if(subscribeRsp.isOk()){ -// //... -// }else { -// //... -// } +//// SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes()); +//// if(subscribeRsp.isOk()){ +//// //... +//// }else { +//// //... +//// } // }else { // System.out.println(); // System.out.println("received transationId : "+ "isok:"+rcv.isOk()); @@ -184,19 +187,19 @@ public class KCCommandDemo { // } // } - { - //0x1F(确认初始位置) - AgvEvent agvEvent = confirmInitialPosition(); - printInfo(agvEvent); - RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); - if(rcv.isOk()){ - System.out.println("0x1F ok"); - }else { - System.out.println(); - System.out.println("0x1F fail"); - System.out.println("received transationId : "+ "isok:"+rcv.isOk()); - } - } +// { +// //0x1F(确认初始位置) +// AgvEvent agvEvent = confirmInitialPosition(); +// printInfo(agvEvent); +// RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); +// if(rcv.isOk()){ +// System.out.println("0x1F ok"); +// }else { +// System.out.println(); +// System.out.println("0x1F fail"); +// System.out.println("received transationId : "+ "isok:"+rcv.isOk()); +// } +// } // { // //0xAE(导航控制导航点控制) diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/QueryRobotStatusRsp.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/QueryRobotStatusRsp.java index 1e6de1b..856780e 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/QueryRobotStatusRsp.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/QueryRobotStatusRsp.java @@ -1,5 +1,6 @@ package org.opentcs.kc.udp.agv.param.function.af; +import java.util.ArrayList; import java.util.List; import org.opentcs.kc.common.byteutils.ByteUtils; @@ -29,22 +30,27 @@ public class QueryRobotStatusRsp { this.locationStatusInfo = new LocationStatusInfo(ByteUtils.copyBytes(src,4,32)); this.runningStatusInfo = new RunningStatusInfo(ByteUtils.copyBytes(src,36,20)); - Integer pointSize = ByteUtils.toInt(src[60]); - Integer edgeSize = ByteUtils.toInt(src[61]); + Integer pointSize = ByteUtils.toInt(src[64]); + Integer edgeSize = ByteUtils.toInt(src[65]); Integer taskByteSize = 12+8*(pointSize+edgeSize); this.taskStatusInfo = new TaskStatusInfo(ByteUtils.copyBytes(src,56,taskByteSize)); + this.batteryStatusInfo = new BatteryStatusInfo(ByteUtils.copyBytes(src,56+taskByteSize,20)); + if(this.abnormal_size>0){ + this.abnormalEventStatusInfoList = new ArrayList<>(); for(int i=0;i0){ + this.actionInfoList = new ArrayList<>(); for(int i=0;i