feat:拍子存放区升降结合传感器

src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java

@@ -63,7 +63,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
             deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧
             double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度
             deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -capSafetyHeight);//机械臂提升至移动拍子的安全高度
-            deviceCommandUtilService.capMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 1); //提升拍子存放区1个位置
+            deviceCommandUtilService.capUpBalance(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区
             Point3D heatAreaCapClawPoint3D = deviceCommandUtilService.getHeatAreaCapClawPointPoint3D(heatModuleId);//获取托盘上方拍子点位
             Point3D heatAreaCapClawSafetyHeightPoint3D = new Point3D(heatAreaCapClawPoint3D.getX(), heatAreaCapClawPoint3D.getY(), heatAreaCapClawPoint3D.getZ() - capSafetyHeight);//加热区拍子上方加上移动拍子的安全高度
             deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawSafetyHeightPoint3D);//将机械臂移动至拍子上方加上移动拍子的安全高度

src/main/java/com/iflytop/gd/app/service/CraftsStepService.java

@@ -195,8 +195,8 @@ public class CraftsStepService {
         double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度
         webSocketService.pushCraftsDebug(CraftsDebugGenerator.generateJson(heatId, "机械臂提升至移动拍子的安全高度", -capSafetyHeight));
         deviceCommandUtilService.gantryZMoveBy(-capSafetyHeight);//机械臂提升至移动拍子的安全高度
-        webSocketService.pushCraftsDebug(CraftsDebugGenerator.generateJson(heatId, "提升拍子存放区1个位置", null));
-        deviceCommandUtilService.capMotorMove(1); //提升拍子存放区1个位置
+        webSocketService.pushCraftsDebug(CraftsDebugGenerator.generateJson(heatId, "提升拍子存放区", null));
+        deviceCommandUtilService.capUpBalance(); //提升拍子存放区
         Point3D heatAreaCapClawPoint3D = deviceCommandUtilService.getHeatAreaCapClawPointPoint3D(heatModuleId);//获取托盘上方拍子点位
         Point3D heatAreaCapClawSafetyHeightPoint3D = new Point3D(heatAreaCapClawPoint3D.getX(), heatAreaCapClawPoint3D.getY(), heatAreaCapClawPoint3D.getZ() - capSafetyHeight);//加热区拍子上方加上移动拍子的安全高度
         webSocketService.pushCraftsDebug(CraftsDebugGenerator.generateJson(heatId, "将机械臂移动至拍子上方加上移动拍子的安全高度", heatAreaCapClawSafetyHeightPoint3D));

src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java

@@ -10,6 +10,8 @@ import com.iflytop.gd.common.enums.AcidPumpDeviceCode;
 import com.iflytop.gd.common.enums.HeatModuleCode;
 import com.iflytop.gd.common.enums.cmd.CmdAxis;
 import com.iflytop.gd.common.enums.data.DevicePositionCode;
+import com.iflytop.gd.hardware.service.GDDeviceStatusService;
+import com.iflytop.gd.hardware.type.IO.InputIOMId;
 import com.iflytop.gd.hardware.type.LiquidDistributionArmDriver;
 import com.iflytop.gd.hardware.type.Servo.LiquidArmMId;
 import lombok.RequiredArgsConstructor;
@@ -25,6 +27,7 @@ public class DeviceCommandUtilService {
     private final DeviceCommandService deviceCommandService;
     private final DevicePositionService devicePositionService;
     private final LiquidDistributionArmDriver liquidDistributionArmDriver;
+    private final GDDeviceStatusService gdDeviceStatusService;
 
     /**
      * 门电机移动
@@ -231,14 +234,38 @@ public class DeviceCommandUtilService {
     }
 
     /**
+     * 将拍子升降区抬升至可以获取的位置
+     */
+    public void capUpBalance() throws Exception {
+        capUpBalance(null, null);
+    }
+
+    /**
+     * 将拍子升降区抬升至可以获取的位置
+     */
+    public void capUpBalance(String cmdId, String cmdCode) throws Exception {
+        int capNum = 0;
+        for (InputIOMId inputIOMId : InputIOMId.values()) {
+            Boolean exist = gdDeviceStatusService.getInputState(inputIOMId);
+            if (exist) {
+                capNum++;
+            }
+        }
+        double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance();
+        double capLiftingHeightSum = (6 - capNum) * capLiftingHeight;
+        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(capLiftingHeightSum);
+        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
+        commandWait(deviceCommandFuture);
+    }
+
+    /**
      * 调整拍子升降电机位置
      *
      * @param num 1为上升1格，-1为下降1格
      */
     public void capMotorMove(String cmdId, String cmdCode, int num) throws Exception {
-        double capHeight = 0.0; //TODO 每个拍子的高度 从数据库中获取
-        //TODO 获取当前拍子升降电机位置
-        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(num * capHeight);
+        double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance();
+        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(num * capLiftingHeight);
         CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
         commandWait(deviceCommandFuture);
     }

src/main/java/com/iflytop/gd/common/enums/data/DevicePositionCode.java

@@ -83,6 +83,11 @@ public enum DevicePositionCode {
      */
     testTubeSafetyHeight(DevicePositionType.DISTANCE, "移动试管安全高度"),
 
+    /**
+     * 拍子升降高度
+     */
+    capLiftingHeight(DevicePositionType.DISTANCE, "拍子升降高度"),
+
     ////////////////////////点
 
     /**