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 8265034..27c2d17 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 @@ -571,10 +571,10 @@ public class LoopbackCommunicationAdapter Step step = command.getStep(); getProcessModel().setState(Vehicle.State.EXECUTING); + System.out.println("start one operation"); if (step.getPath() == null) { LOG.debug("-Starting operation..."); //无可执行路径，检查是否需要执行动作 -// operationSimulation(command, 1); operationExec(command); } else { getProcessModel().getVelocityController().addWayEntry( @@ -656,12 +656,14 @@ public class LoopbackCommunicationAdapter //若否，更新当前车辆位置，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。 LAST_PASSED_POINT = currentPoint; - LOG.debug("-Movement finished."); + + System.out.println("-emptyOperation:" + command.hasEmptyOperation()); if (!command.hasEmptyOperation()) { //执行AGV动作 LOG.debug("-Starting operation..."); operationExec(command); } else { + LOG.debug("-Movement Finishing command."); //完成当前命令 finishMoveCmd(command); //执行下一个命令 @@ -724,91 +726,103 @@ public class LoopbackCommunicationAdapter */ private void operationExec(MovementCommand command) { - //todo------------------- + System.out.println("operationExec00000000000000"); if (ACTION_EXECUTION_STATUS) { - //递归进入 + System.out.println("operationExec-----------"); //todo 读取动作执行状态 byte[] value = ReadValue.command(); int execStatus = value[0]; + System.out.println(); System.out.println("-execStatus:" + execStatus); if (execStatus == 1) { - //动作执行完成,执行重置任务 + //递归 + getExecutor().schedule( + this::readActionState, + 200, + TimeUnit.MILLISECONDS + ); - resetOperation(); + LOG.debug("-Operation exec finished."); - LOG.debug("-Operation exec finished."); - ACTION_EXECUTION_STATUS = false; + ActImmediately.reset(); - finishMoveCmd(command); + //todo 读取动作执行状态 + getExecutor().schedule( + this::readActionState, + 200, + TimeUnit.MILLISECONDS + ); + + ACTION_EXECUTION_STATUS = false; + + finishMoveCmd(command); + + nextCommand(); } else { LOG.debug("-Operation exec 1111111."); //递归 getExecutor().schedule( () -> operationExec(command), - SIMULATION_PERIOD, + 500, TimeUnit.MILLISECONDS ); } } else { + System.out.println("operationExec==========="); //首次进入 ACTION_EXECUTION_STATUS = true; //获取动作 String operation = command.getOperation(); - LOG.debug("-Operation exec start."); + LOG.debug("-Operation exec start:" + operation); //下发立即动作指令 if ("LIFT".equals(operation)) { LOG.debug("-Operation exec LoadOperation."); - resetOperation(); - float height = 1.1f; byte modeOfMovement = 0x01; - //抬升叉齿指令 - ActImmediately.command(height, modeOfMovement); + allotsOperation(height, modeOfMovement); } else if ("DECLINE".equals(operation)) { LOG.debug("-Operation exec UnloadOperation."); - resetOperation(); float height = 0.1f; byte modeOfMovement = 0x02; //降叉齿指令 - ActImmediately.command(height, modeOfMovement); + allotsOperation(height, modeOfMovement); } else if (operation.equals(this.getRechargeOperation())) { LOG.debug("-Operation exec RechargeOperation."); - resetOperation(); +// allotsOperation(); //充电指令 } else { - LOG.debug("-Operation exec 2222222."); + LOG.debug("-Operation exec :" + operation); //动作未定义，不执行 ACTION_EXECUTION_STATUS = false; nextCommand(); } - LOG.debug("-Operation exec 3333333."); - + LOG.debug("-Operation exec 222222."); //递归 getExecutor().schedule( () -> operationExec(command), - SIMULATION_PERIOD, + 500, TimeUnit.MILLISECONDS ); } @@ -817,18 +831,44 @@ public class LoopbackCommunicationAdapter //todo------------------- } - private void resetOperation() { + private void allotsOperation(float height, byte modeOfMovement) { + + LOG.debug("allotsOperation111111"); + + //todo 读取动作执行状态 + getExecutor().schedule( + this::readActionState, + 200, + TimeUnit.MILLISECONDS + ); ActImmediately.reset(); + + //todo 读取动作执行状态 + getExecutor().schedule( + this::readActionState, + 200, + TimeUnit.MILLISECONDS + ); + //抬升叉齿指令 + ActImmediately.command(height, modeOfMovement); + } + + /** + * 读取动作状态 + */ + private void readActionState() { + //todo 读取动作执行状态 byte[] value = ReadValue.command(); - int execStatus = value[0]; + int execCmd = value[0]; - while (execStatus == 2) { - LOG.debug("-resetOperation error"); - byte[] value1 = ReadValue.command(); - execStatus = value1[0]; + if (execCmd == 2) { + getExecutor().schedule( + () -> readActionState(), + 200, + TimeUnit.MILLISECONDS + ); } - } /** 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 203f9d3..fd43640 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 @@ -4,6 +4,8 @@ import java.nio.ByteOrder; import java.util.ArrayList; import java.util.List; import org.opentcs.kc.common.byteutils.ByteUtils; +import org.opentcs.kc.udp.Service.ActImmediately; +import org.opentcs.kc.udp.Service.ReadValue; 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; @@ -225,19 +227,90 @@ 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 = 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()); +// } +// } + +// { +// +// //todo 读取动作执行状态 +// byte[] value = ReadValue.command(); +// int execStatus = value[0]; +// +// Thread.sleep(200); +// +// while (execStatus == 2) { +// byte[] value1 = ReadValue.command(); +// execStatus = value1[0]; +// } +// +// +// ActImmediately.reset(); +// +// //todo 读取动作执行状态 +// byte[] value2 = ReadValue.command(); +// int execStatus2 = value2[0]; +// +// Thread.sleep(200); +// +// while (execStatus2 == 2) { +// byte[] value1 = ReadValue.command(); +// execStatus2 = value1[0]; +// } +// +// float height = 1.1f; +// byte modeOfMovement = 0x01; +// +// //抬升叉齿指令 +// ActImmediately.command(height, modeOfMovement); +// } + + { + ActImmediately.reset(); + } + +// { +// +// //todo 读取动作执行状态 +// byte[] value = ReadValue.command(); +// int execStatus = value[0]; +// +// Thread.sleep(200); +// +// while (execStatus == 2) { +// byte[] value1 = ReadValue.command(); +// execStatus = value1[0]; +// } +// +// +// ActImmediately.reset(); +// +// //todo 读取动作执行状态 +// byte[] value2 = ReadValue.command(); +// int execStatus2 = value2[0]; +// +// Thread.sleep(200); +// +// while (execStatus2 == 2) { +// byte[] value1 = ReadValue.command(); +// execStatus2 = value1[0]; +// } +// +// float height = 0.1f; +// byte modeOfMovement = 0x02; +// +// ActImmediately.command(height, modeOfMovement); +// } @@ -338,26 +411,10 @@ public class KCCommandDemo { public static AgvEvent actionNow() { AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY); //TODO 构建 - Integer actionID = 5; + Integer actionID = 102; // //构建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( +// Action actionLiftUp = new Action( // ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节 // (byte) 0x02, //只能执行当前动作 // actionID, //actionID @@ -365,10 +422,26 @@ public class KCCommandDemo { // ActionSet.fork0x12( // 1.1f, //升降高度 // (byte) 0x01, //1上升 2下降 -// (byte)0x00 //01 开始 任务 +// (byte)0x01 //01 开始 任务 // ) -// ); -// agvEvent.setBody(actionReset.toBytes()); +// ); +// agvEvent.setBody(actionLiftUp.toBytes()); + + + +// //重置 空指令 + Action actionReset = new Action( + ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节 + (byte) 0x02, //只能执行当前动作 + actionID, //actionID + ActionSet.fork0x12_paramsize, + ActionSet.fork0x12( + 0.0f, //升降高度 + (byte) 0x01, //1上升 2下降 + (byte)0x00 //01 开始 任务 + ) + ); + agvEvent.setBody(actionReset.toBytes()); // // //构建Action指令 // Action actionLiftDown = new Action( 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 index fa64739..67e777f 100644 --- 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 @@ -9,7 +9,7 @@ import org.opentcs.kc.udp.io.UDPClient; public class ActImmediately extends BaseCommand{ - private static Integer actionId = 20; + private static Integer actionId = 115; /** * decs: 立即动作指令 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 9368ea4..dc78df0 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 = 25; + private static int currentMaxiOrderId = 100; /** @@ -78,7 +78,7 @@ public class HybridNavigation Path[] paths = new Path[]{ new Path( - pathSerialNum, pathID, 0f, (byte) 0x00, (byte) 0x01, ByteUtils.usintTo1Byte(0), 1f, 1f, null + pathSerialNum, pathID, 0f, (byte) 0x00, (byte) 0x00, ByteUtils.usintTo1Byte(0), 1f, 1f, null ), }; pathSerialNum += 2; 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 index b4daadb..30a79ca 100644 --- 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 @@ -35,7 +35,7 @@ public class ReadValue extends BaseCommand{ RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); if (rcv.isOk()) { System.out.println(); - System.out.println("-received " + "isok:" + rcv.isOk() + " dataBytes:"); + System.out.println("-received readvalue" + "isok:" + rcv.isOk() + " dataBytes:"); printInfo(rcv); ReadRsp readRsp = new ReadRsp(rcv.getDataBytes()); return readRsp.dataValue; 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 47c497b..dc051f8 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 @@ -154,10 +154,6 @@ public enum UDPClient { public void subscribeKC(RcvEventPackage rcv, byte[] body) { if (rcv.isOk()) { - Date date = new Date(); - - System.out.println("time:" + date.getTime()); - for (byte b : body) { System.out.print(byteToHex(b) + " "); }