diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java index be614c2..8265034 100644 --- a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java @@ -34,11 +34,13 @@ import org.opentcs.drivers.vehicle.SimVehicleCommAdapter; import org.opentcs.drivers.vehicle.VehicleCommAdapter; import org.opentcs.drivers.vehicle.VehicleProcessModel; import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO; +import org.opentcs.kc.udp.Service.ActImmediately; import org.opentcs.kc.udp.Service.ConfirmRelocation; import org.opentcs.kc.udp.Service.HybridNavigation; import org.opentcs.kc.udp.Service.ManualPosition; import org.opentcs.kc.udp.Service.QryRobotRunStatus; import org.opentcs.kc.udp.Service.QryRobotStatus; +import org.opentcs.kc.udp.Service.ReadValue; import org.opentcs.kc.udp.Service.SubCargoStatus; import org.opentcs.kc.udp.Service.SubRobotStatue; import org.opentcs.kc.udp.Service.SwitchAutomaticMode; @@ -148,6 +150,10 @@ public class LoopbackCommunicationAdapter * 用于判断是否切换执行订单 */ private static String uniqueOrderName; + /** + * 动作执行状态 + */ + private static boolean ACTION_EXECUTION_STATUS; /** * Creates a new instance. @@ -320,7 +326,7 @@ public class LoopbackCommunicationAdapter System.out.println("send cmd print end"); - //AGV控制器执行命令 + //AGV控制器执行命令------todo 待修改-》opentcs首次会下两个指令，未进入线程中不进行任务阻塞下发。 HybridNavigation.command(orderName, sourcePoint, destinationPointName, operation); //检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。 @@ -331,7 +337,7 @@ public class LoopbackCommunicationAdapter if (uniqueOrderName == null || !uniqueOrderName.equals(orderName)) { //设置唯一订单名称 uniqueOrderName = orderName; - //记录订单起点 + //记录订单起点---更新复位后可 LAST_PASSED_POINT = sourcePoint; } @@ -567,7 +573,7 @@ public class LoopbackCommunicationAdapter if (step.getPath() == null) { LOG.debug("-Starting operation..."); - //动作执行待完成 + //无可执行路径，检查是否需要执行动作 // operationSimulation(command, 1); operationExec(command); } else { @@ -718,6 +724,111 @@ public class LoopbackCommunicationAdapter */ private void operationExec(MovementCommand command) { + //todo------------------- + + + if (ACTION_EXECUTION_STATUS) { + //递归进入 + + //todo 读取动作执行状态 + byte[] value = ReadValue.command(); + int execStatus = value[0]; + + System.out.println("-execStatus:" + execStatus); + + if (execStatus == 1) { + //动作执行完成,执行重置任务 + + resetOperation(); + + LOG.debug("-Operation exec finished."); + ACTION_EXECUTION_STATUS = false; + + finishMoveCmd(command); + } else { + LOG.debug("-Operation exec 1111111."); + //递归 + getExecutor().schedule( + () -> operationExec(command), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + } else { + //首次进入 + ACTION_EXECUTION_STATUS = true; + + //获取动作 + String operation = command.getOperation(); + LOG.debug("-Operation exec start."); + + //下发立即动作指令 + if ("LIFT".equals(operation)) { + + LOG.debug("-Operation exec LoadOperation."); + + resetOperation(); + + float height = 1.1f; + byte modeOfMovement = 0x01; + + //抬升叉齿指令 + ActImmediately.command(height, modeOfMovement); + + } else if ("DECLINE".equals(operation)) { + + LOG.debug("-Operation exec UnloadOperation."); + + resetOperation(); + + float height = 0.1f; + byte modeOfMovement = 0x02; + + //降叉齿指令 + ActImmediately.command(height, modeOfMovement); + + } else if (operation.equals(this.getRechargeOperation())) { + + LOG.debug("-Operation exec RechargeOperation."); + + resetOperation(); + + //充电指令 + + } else { + LOG.debug("-Operation exec 2222222."); + + //动作未定义，不执行 + ACTION_EXECUTION_STATUS = false; + nextCommand(); + } + + LOG.debug("-Operation exec 3333333."); + + //递归 + getExecutor().schedule( + () -> operationExec(command), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + + + //todo------------------- + } + + private void resetOperation() { + ActImmediately.reset(); + //todo 读取动作执行状态 + byte[] value = ReadValue.command(); + int execStatus = value[0]; + + while (execStatus == 2) { + LOG.debug("-resetOperation error"); + byte[] value1 = ReadValue.command(); + execStatus = value1[0]; + } + } /** @@ -824,7 +935,7 @@ public class LoopbackCommunicationAdapter } /** - * 结束移动命令。 + * 结束命令。 * @param command 指令 */ private void finishMoveCmd(MovementCommand command) { @@ -834,6 +945,8 @@ public class LoopbackCommunicationAdapter getProcessModel().setState(Vehicle.State.IDLE); //清除订单对应唯一ID HybridNavigation.delUniqueOrderID(command); + + LOG.debug("-callback wms"); } //如果传入指令和移动指令队列第一条数据相同 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 190d990..203f9d3 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 @@ -338,45 +338,46 @@ public class KCCommandDemo { public static AgvEvent actionNow() { AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY); //TODO 构建 - Integer orderId = 1; + Integer actionID = 5; // //构建Action指令 -// Action actionLiftUp = new Action( + Action actionLiftUp = new Action( + ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节 + (byte) 0x02, //只能执行当前动作 + actionID, //actionID + ActionSet.fork0x12_paramsize, + ActionSet.fork0x12( + 1.1f, //升降高度 + (byte) 0x01, //1上升 2下降 + (byte)0x01 //01 开始 任务 + ) + ); + agvEvent.setBody(actionLiftUp.toBytes()); + + + +// //重置 空指令 +// Action actionReset = new Action( // ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节 // (byte) 0x02, //只能执行当前动作 -// 1, //actionID +// actionID, //actionID // ActionSet.fork0x12_paramsize, // ActionSet.fork0x12( // 1.1f, //升降高度 // (byte) 0x01, //1上升 2下降 -// (byte)0x01 //01 开始 任务 +// (byte)0x00 //01 开始 任务 // ) -// ); -// agvEvent.setBody(actionLiftUp.toBytes()); - - - - //重置 空指令 - Action actionReset = new Action( - ActionSet.cancel0x03, //机器人移动到正常点 停止，2个字节 - (byte) 0x02, //只能执行当前动作 - 2, //actionID - ActionSet.cancle0x03_paramsize, - ActionSet.cancel0x12( - 1, //订单id - (byte)0x01 //1 立即停止， 2 正常移动到点再停止 - ) - ); - agvEvent.setBody(actionReset.toBytes()); +// ); +// agvEvent.setBody(actionReset.toBytes()); // // //构建Action指令 // Action actionLiftDown = new Action( // ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节 // (byte) 0x02, //只能执行当前动作 -// 1, //actionID +// actionID, //actionID // ActionSet.fork0x12_paramsize, // ActionSet.fork0x12( -// 1.1f, //升降高度 +// 0.1f, //升降高度 // (byte) 0x02, //1上升 2下降 // (byte)0x01 //01 开始 任务 // ) diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java new file mode 100644 index 0000000..fa64739 --- /dev/null +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java @@ -0,0 +1,92 @@ +package org.opentcs.kc.udp.Service; + +import org.opentcs.kc.udp.agv.param.AgvEvent; +import org.opentcs.kc.udp.agv.param.AgvEventConstant; +import org.opentcs.kc.udp.agv.param.function.navigation.Action; +import org.opentcs.kc.udp.agv.param.function.navigation.ActionSet; +import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; +import org.opentcs.kc.udp.io.UDPClient; + +public class ActImmediately extends BaseCommand{ + + private static Integer actionId = 20; + + /** + * decs: 立即动作指令 + * 指令：0xB2 + * author: caixiang + * date: 2025/1/17 16:25 + */ + private static AgvEvent actionNow(float execuHeight, byte modeOfMovement) { + AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY); + //TODO 构建 + + //构建Action指令 + Action actionLiftUp = new Action( + ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节 + (byte) 0x02, //只能执行当前动作 + actionId, //动作ID + ActionSet.fork0x12_paramsize, + ActionSet.fork0x12( + execuHeight, //升降高度 + modeOfMovement, //1上升 2下降 + (byte)0x01 //01 开始任务 + ) + ); + + actionId++; + agvEvent.setBody(actionLiftUp.toBytes()); + return agvEvent; + } + + private static AgvEvent resetCmd() { + AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY); + //重置 空指令 + Action actionReset = new Action( + ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节 + (byte) 0x02, //只能执行当前动作 + actionId, //actionID + ActionSet.fork0x12_paramsize, + ActionSet.fork0x12( + 0.1f, //升降高度 + (byte) 0x01, //1上升 2下降 + (byte)0x00 //00 无任务 + ) + ); + agvEvent.setBody(actionReset.toBytes()); + + actionId++; + agvEvent.setBody(actionReset.toBytes()); + return agvEvent; + } + + + public static void command(float execuHeight, byte modeOfMovement){ + //0xB2(立即动作指令) + AgvEvent agvEvent = actionNow(execuHeight, modeOfMovement); + printInfo(agvEvent); + RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); + if(rcv.isOk()){ + System.out.println("0xB2 forklift ok"); + }else { + System.out.println(); + System.out.println("0xB2 forklift fail"); + System.out.println("received transationId : "+ "isok:"+rcv.isOk()); + } + } + + public static void reset(){ + //0xB2(立即动作指令) + AgvEvent agvEvent = resetCmd(); + printInfo(agvEvent); + RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); + if(rcv.isOk()){ + System.out.println("-0xB2 reset ok"); + }else { + System.out.println(); + System.out.println("-0xB2 reset fail"); + System.out.println("-received transationId : "+ "isok:"+rcv.isOk()); + } + } + +} diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java index 8afbc59..9368ea4 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java @@ -49,7 +49,7 @@ public class HybridNavigation /** * 订单映射最大订单ID. */ - private static int currentMaxiOrderId = 0; + private static int currentMaxiOrderId = 25; /** diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ReadValue.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ReadValue.java new file mode 100644 index 0000000..b4daadb --- /dev/null +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ReadValue.java @@ -0,0 +1,50 @@ +package org.opentcs.kc.udp.Service; + +import java.util.ArrayList; +import java.util.List; +import org.opentcs.kc.udp.agv.param.AgvEvent; +import org.opentcs.kc.udp.agv.param.AgvEventConstant; +import org.opentcs.kc.udp.agv.param.function.read.ReadParam; +import org.opentcs.kc.udp.agv.param.function.read.ReadRsp; +import org.opentcs.kc.udp.agv.param.function.read.ReadStrValue; +import org.opentcs.kc.udp.agv.param.function.read.ReadValueMember; +import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; +import org.opentcs.kc.udp.io.UDPClient; + +public class ReadValue extends BaseCommand{ + + private static AgvEvent readValue() { + AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_READ); + List readValueMemberList = new ArrayList<>(); + ReadValueMember readValueMember1 = new ReadValueMember(Short.valueOf("10"), Short.valueOf("1")); + readValueMemberList.add(readValueMember1); + + List readStrValueList = new ArrayList<>(); + ReadStrValue readStrValue = new ReadStrValue("Lift", 1, readValueMemberList); + readStrValueList.add(readStrValue); + + ReadParam readParam = new ReadParam(agvEvent.getTransationId(), readStrValueList); + agvEvent.setBody(readParam.toBytes()); + + return agvEvent; + } + + public static byte[] command() { + AgvEvent agvEvent = readValue(); + printInfo(agvEvent); + RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); + if (rcv.isOk()) { + System.out.println(); + System.out.println("-received " + "isok:" + rcv.isOk() + " dataBytes:"); + printInfo(rcv); + ReadRsp readRsp = new ReadRsp(rcv.getDataBytes()); + return readRsp.dataValue; + } + else { + System.out.println(); + System.out.println("-received transationId : " + "isok:" + rcv.isOk()); + throw new RuntimeException("-read failed"); + } + } + +}