调试货叉指令

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");
     }
 
     //如果传入指令和移动指令队列第一条数据相同