取出托盘前置判断

src/main/java/com/iflytop/sgs/app/cmd/selftest/TrayOutCommand.java

@@ -1,9 +1,11 @@
 package com.iflytop.sgs.app.cmd.selftest;
 
+import cn.hutool.core.lang.Assert;
 import com.iflytop.sgs.app.core.BaseCommandHandler;
 import com.iflytop.sgs.app.model.bo.Point3D;
 import com.iflytop.sgs.app.model.dto.CmdDTO;
 import com.iflytop.sgs.app.service.api.DevicePositionService;
+import com.iflytop.sgs.app.service.device.DeviceSensorService;
 import com.iflytop.sgs.app.service.device.DeviceStateService;
 import com.iflytop.sgs.app.service.device.module.DoorModuleService;
 import com.iflytop.sgs.app.service.device.module.HeatModuleService;
@@ -34,6 +36,7 @@ public class TrayOutCommand extends BaseCommandHandler {
     private final HeatModuleService heatModuleService;
     private final DevicePositionService devicePositionService;
     private final DeviceStateService deviceStateService;
+    private final DeviceSensorService deviceSensorService;
 
 
     @Override
@@ -47,18 +50,15 @@ public class TrayOutCommand extends BaseCommandHandler {
             throw new AppException(ResultCode.INVALID_PARAMETER);
         }
         HeatModuleCode targetHeatModuleCode = HeatModuleCode.valueOf(targetHeatModuleCodeStr);
-        Double doorOpenPosition = devicePositionService.getPosition(DevicePositionCode.doorOpenPosition).getDistance();//获取开门位置
         Point3D targetHeatModuleTrayClawPoint3D = heatModuleService.getHeatModuleTrayClawPoint3D(targetHeatModuleCode);//获取目标加热模块托盘夹取点
         Double transferModuleXPickTrayMoveDistance = devicePositionService.getPosition(DevicePositionCode.transferModuleXPickTrayMoveDistance).getDistance();//获取转运模块X轴拿取托盘进出卡槽移动距离
         Point3D feedAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.feedAreaTrayPoint).getPoint3D();//获取上料区托盘夹爪位置点
         deviceStateService.getCommandMutexState().get().setTrayOut(true);
         return runAsync(() -> {
             try{
-                if (!deviceStateService.getDeviceState().getDoorModule().isOpen()) {
-                    doorModuleService.doorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), doorOpenPosition);
-                    deviceStateService.getDeviceState().getDoorModule().setOpen(true);
-                }
-                solutionModuleService.solutionMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);
+                boolean heatModuleTrayExist = deviceSensorService.getTrayStateByHeatModuleCode(targetHeatModuleCode);//目标加热模块有无托盘
+                Assert.isTrue(heatModuleTrayExist, () -> new AppException(ResultCode.TARGET_HEAT_MODULE_NO_TRAY));//提示被占用
+                solutionModuleService.solutionMotorMoveZero();
                 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getX() + transferModuleXPickTrayMoveDistance);//将X轴移动至目标加热模块托盘夹取点 + 进出卡槽移动距离
                 transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getZ());//Z轴下降至夹取点，使托盘落入石墨加热盘
                 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -transferModuleXPickTrayMoveDistance);//X轴进入卡槽