动作相关指令

opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java

@@ -241,44 +241,47 @@ public class KCCommandDemo {
 //        }
 //      }
 
-//    {
-//
-//      //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();
+
+      System.out.println("00000");
+      Thread.sleep(100);
+      ActImmediately.reset();
+
+      //todo 读取动作执行状态
+      int execStatus = 2;
+      while (execStatus == 2) {
+        Thread.sleep(200);
+        byte[] value = ReadValue.command();
+        execStatus = value[0];
+      }
+
+      System.out.println("111111111");
+      Thread.sleep(100);
+      ActImmediately.reset();
+
+      //todo 读取动作执行状态
+      int execStatus2 = 2;
+      while (execStatus2 == 2) {
+        Thread.sleep(200);
+        byte[] value = ReadValue.command();
+        execStatus2 = value[0];
+      }
+
+      float height = 1.1f;
+      byte modeOfMovement = 0x01;
+
+      Thread.sleep(100);
+      //抬升叉齿指令
+      ActImmediately.command(height, modeOfMovement);
+
+      Thread.sleep(100);
+      ActImmediately.reset();
     }
 
+//    {
+//            ActImmediately.reset();
+//    }
+
 //    {
 //
 //      //todo 读取动作执行状态

opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java

@@ -9,7 +9,9 @@ import org.opentcs.kc.udp.io.UDPClient;
 
 public class ActImmediately extends BaseCommand{
 
-  private static Integer actionId = 115;
+  private static Integer actionId = 1;
+
+  public static boolean ACTION_EXECUTE_STATE = false;
 
   /**
    * decs: 立即动作指令
@@ -89,4 +91,50 @@ public class ActImmediately extends BaseCommand{
     }
   }
 
+  /**
+   * 读取动作状态
+   */
+  private static void readActionState() {
+
+    try {
+      Thread.sleep(200);
+
+      //todo 读取动作执行状态
+      byte[] value = ReadValue.command();
+      int execStatus = value[0];
+
+      System.out.println();
+      System.out.println("-execStatus:" + execStatus);
+
+      while (execStatus == 2) {
+        Thread.sleep(200);
+        byte[] value1 = ReadValue.command();
+        execStatus = value1[0];
+      }
+
+      Thread.sleep(100);
+    } catch (Exception e) {
+      throw new RuntimeException("readActionState:" + e);
+    }
+  }
+
+  public static void allotsOperation(float height, byte modeOfMovement)  {
+
+    //读取动作执行状态
+    readActionState();
+    //重置
+    ActImmediately.reset();
+    //读取动作执行状态
+    readActionState();
+    //执行叉齿指令
+    ActImmediately.command(height, modeOfMovement);
+    //读取动作执行状态
+    readActionState();
+    //重置
+    ActImmediately.reset();
+    //读取重置状态
+    readActionState();
+
+    ACTION_EXECUTE_STATE = false;
+  }
 }

opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java

@@ -49,7 +49,7 @@ public class HybridNavigation
   /**
    * 订单映射最大订单ID.
    */
-  private static int currentMaxiOrderId = 100;
+  private static int currentMaxiOrderId = 1;
 
 
   /**

opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubRobotStatue.java

@@ -14,7 +14,14 @@ public class SubRobotStatue
     extends
       BaseCommand {
 
+  /**
+   * 上报时间间隔
+   */
   public static final int intervalTime = 1000;
+  /**
+   * 订阅时长
+   */
+  public static int subDuration = intervalTime * 3;
 
   /**
    * decs: 下发订阅信息
@@ -25,7 +32,7 @@ public class SubRobotStatue
   public static AgvEvent issueSubscribe() {
     List<SubscribeInfo> subscribeInfoList = new ArrayList<>();
     SubscribeInfo subscribeInfo = new SubscribeInfo(
-        new byte[]{(byte) 0xaf, (byte) 0x00}, (short) intervalTime, 10000
+        new byte[]{(byte) 0xaf, (byte) 0x00}, (short) intervalTime, subDuration
     );
 //    SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xb0,(byte)0x00}, (short) 100,1000);
     subscribeInfoList.add(subscribeInfo);

opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java

@@ -124,6 +124,10 @@ public enum UDPClient {
       Package mbPackage = event.toBytes();
       AsyncFuture<Package> add = SendedList.add(mbPackage.getTransationId(), null);
 
+      if(this.conn == null || !this.conn.isActive()) {//如果连接断开，就重新连接
+        initClient();
+      }
+
       this.conn.writeAndFlush(
           new DatagramPacket(
               byteArrayToByteBuf(mbPackage.getBody()),