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 e0b9766..9162d48 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 @@ -37,28 +37,28 @@ import org.opentcs.kc.udp.io.UDPClient; 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()){ - System.out.println(); - System.out.println("received transationId : "+ "isok:"+rcv.isOk()); - for (byte b:rcv.getValue()){ - System.out.print(byteToHex(b)+" "); - } - System.out.println(); - System.out.println("---------------------"); - for (byte c:rcv.getDataBytes()){ - System.out.print(byteToHex(c)+" "); - } - QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes()); - }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()){ +// System.out.println(); +// System.out.println("received transationId : "+ "isok:"+rcv.isOk()); +// for (byte b:rcv.getValue()){ +// System.out.print(byteToHex(b)+" "); +// } +// System.out.println(); +// System.out.println("---------------------"); +// for (byte c:rcv.getDataBytes()){ +// System.out.print(byteToHex(c)+" "); +// } +// QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes()); +// }else { +// System.out.println(); +// System.out.println("received transationId : "+ "isok:"+rcv.isOk()); +// } +// } // { // //0xB0(查询载货状态) @@ -224,6 +224,34 @@ public class KCCommandDemo { } } + { + //0xB2(立即动作指令) + AgvEvent agvEvent = actionNow(); + printInfo(agvEvent); + RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); + if(rcv.isOk()){ + System.out.println("0xB2 ok"); + }else { + System.out.println(); + System.out.println("0xB2 fail"); + System.out.println("received transationId : "+ "isok:"+rcv.isOk()); + } + } + + { + //0xB2(立即动作指令) + AgvEvent agvEvent = navigationControl(); + printInfo(agvEvent); + RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); + if(rcv.isOk()){ + System.out.println("0xAE ok"); + }else { + System.out.println(); + System.out.println("0xAE fail"); + System.out.println("received transationId : "+ "isok:"+rcv.isOk()); + } + } + } @@ -312,6 +340,60 @@ public class KCCommandDemo { return agvEvent; } + /** + * decs: 立即动作指令 + * 指令:0xB2 + * author: caixiang + * date: 2025/1/17 16:25 + */ + public static AgvEvent actionNow() { + AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY); + //TODO 构建 + Integer orderId = 1; + + //构建Action指令 + Action actionLiftUp = new Action( + ActionSet.stop0x00, //机器人移动到正常点 停止,2个字节 + (byte) 0x02, //只能执行当前动作 + 1, //actionID + ActionSet.fork0x12_paramsize, + ActionSet.fork0x12( + 1.1f, //升降高度 + (byte) 0x01, //1上升 2下降 + (byte)0x01 //01 开始 任务 + ) + ); + agvEvent.setBody(actionLiftUp.toBytes()); +// +// //重置 空指令 +// Action actionReset = new Action( +// ActionSet.stop0x00, //机器人移动到正常点 停止,2个字节 +// (byte) 0x02, //只能执行当前动作 +// 2, //actionID +// ActionSet.cancle0x03_paramsize, +// ActionSet.cancel0x12( +// 1, //订单id +// (byte)0x01 //1 立即停止, 2 正常移动到点再停止 +// ) +// ); +// agvEvent.setBody(actionReset.toBytes()); +// +// //构建Action指令 +// Action actionLiftDown = new Action( +// ActionSet.stop0x00, //机器人移动到正常点 停止,2个字节 +// (byte) 0x02, //只能执行当前动作 +// 1, //actionID +// ActionSet.fork0x12_paramsize, +// ActionSet.fork0x12( +// 1.1f, //升降高度 +// (byte) 0x02, //1上升 2下降 +// (byte)0x01 //01 开始 任务 +// ) +// ); +// agvEvent.setBody(actionLiftDown.toBytes()); + return agvEvent; + } + /** * decs: 导航控制 * 指令:0xAE diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/ActionSet.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/ActionSet.java index d76058a..66d6a56 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/ActionSet.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/ActionSet.java @@ -11,6 +11,9 @@ public class ActionSet { } public static byte stop0x01 = 0x01; + public static byte stop0x00 = 0x00; + + public static byte recover0x02 = 0x02; public static byte cancelTask0x03 = 0x03; public static byte forkliftElevation0x12 = 0x12; @@ -18,6 +21,8 @@ public class ActionSet { public static Integer stop0x01_paramsize = 8; + public static Integer fork0x12_paramsize = 8; + public static Integer cancle0x03_paramsize = 8; public static Integer recover0x02_paramsize = 8; public static Integer cancelTask0x03_paramsize = 8; public static Integer forkliftElevation0x12_paramsize = 8; @@ -31,6 +36,32 @@ public class ActionSet { return byteBuf.array(); } + /** + * high : 升降高度 + * type: 1 上升 ; 2 下降 + * command : 0:无任务 + * 1:开始任务 + * 2:暂停任务 + * 3:继续任务 + * 4:放弃任务 + * */ + public static byte[] fork0x12(Float high, byte type,byte command) { + ByteBuf byteBuf = Unpooled.buffer(8); + byteBuf.writeBytes(ByteUtils.floatToBytes(high)); + byteBuf.writeByte((byte)0x01); + byteBuf.writeByte(type); + byteBuf.writeByte(command); + byteBuf.writeByte((byte)0x00); + return byteBuf.array(); + } + + public static byte[] cancel0x12(Integer orderId, byte type) { + ByteBuf byteBuf = Unpooled.buffer(8); + byteBuf.writeBytes(ByteUtils.intToBytes(orderId, 1)); + byteBuf.writeByte(type); + return byteBuf.array(); + } + public static byte[] recover0x02(Integer orderId, Integer taskKey) { ByteBuf byteBuf = Unpooled.buffer(8); diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java index dba68fb..59a6dbd 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java @@ -31,7 +31,7 @@ import org.slf4j.LoggerFactory; public enum UDPClient { //如果要配置多个链接, local1 local2 .... 这样排下去好了 - localAGV("50", "127.0.0.1", 17804, 55678), + localAGV("50", "192.168.124.124", 17804, 55678), ; // 服务端用户名+密码+地址+端口 diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/server/main.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/server/main.java index 698960b..e873508 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/server/main.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/server/main.java @@ -12,7 +12,7 @@ public class main { //size 148 ,contentSize = 148-28 = 120 - String errSrc = "d4 97 44 9c cb cf 0b 4c 95 51 d8 61 70 f1 e7 94 01 01 00 00 10 af 00 00 00 68 00 00 00 00 00 00 53 3a 48 bd 32 fb 7b bc ca 13 03 ba 01 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 01 00 00 00 00 00 00 05 00 00 00 01 00 00 00 01 01 00 00 02 00 00 00 02 00 00 00 01 00 00 00 0c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00\n"; + String errSrc = "d4 97 44 9c cb cf 0b 4c 95 51 d8 61 70 f1 e7 94 01 01 00 00 10 af 00 00 00 68 00 00 00 00 00 00 53 3a 48 bd 32 fb 7b bc ca 13 03 ba 01 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 01 00 00 00 00 00 00 05 00 00 00 01 00 00 00 01 01 00 00 02 00 00 00 02 00 00 00 01 00 00 00 0c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00"; String okSrc = "d4 97 44 9c cb cf 0b 4c 95 51 d8 61 70 f1 e7 94 01 01 03 00 10 af 00 00 64 00 00 00 01 00 00 00 73 1c 30 3d 66 ba 03 3d d9 51 59 bb 00 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 0a 01 02 00 00 00 00 00 00 00 00 00 73 1c 30 3d 66 ba 03 3d d9 51 59 bb 00 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 "; String[] s = errSrc.split(" "); System.out.println("size: " + s.length);