feat:移动时路线避障

src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java

@@ -6,6 +6,7 @@ import com.iflytop.gd.app.model.bo.status.device.HeatModuleState;
 import com.iflytop.gd.app.model.bo.status.device.TrayState;
 import com.iflytop.gd.app.model.dto.CmdDTO;
 import com.iflytop.gd.app.service.api.DevicePositionService;
+import com.iflytop.gd.app.service.device.ActionCommandService;
 import com.iflytop.gd.app.service.device.DeviceCommandTempUtilService;
 import com.iflytop.gd.app.service.device.DeviceStateService;
 import com.iflytop.gd.app.service.device.module.CapModuleService;
@@ -31,13 +32,13 @@ import java.util.concurrent.CompletableFuture;
 @RequiredArgsConstructor
 @CommandMapping("move_to_heat_area")//业务指令注解
 public class MoveToHeatAreaCommand extends BaseCommandHandler {
-    private final HeatModuleService heatModuleService;
-    private final CapModuleService capModuleService;
     private final SolutionModuleService solutionModuleService;
     private final GantryModuleService gantryModuleService;
+    private final CapModuleService capModuleService;
     private final DevicePositionService devicePositionService;
     private final DeviceStateService deviceStateService;
-    private final DeviceCommandTempUtilService deviceCommandTempUtilService;
+    private final ActionCommandService actionCommandService;
+
 
     @Override
     public CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception {
@@ -57,7 +58,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
         try {
             deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(true);
             HeatModuleState heatModuleState = deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode);
-            if(heatModuleState.getTrayStatus() == 1){
+            if (heatModuleState.getTrayStatus() == 1) {
                 throw new AppException(ResultCode.TARGET_HEAT_MODULE_OCCUPIED);
             }
 //            Boolean heatModuleTray = heatModuleService.heatModuleSensorState(heatModuleCode);
@@ -76,63 +77,17 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
             throw e;
         }
 
-        double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //获取拍子夹取距离
-        double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离
-        double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离
-        double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离
-        double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //托盘升降抬升距离
-        double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
-        double heatModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleCapMoveHeight).getDistance();//加热模块拍子移动高度
-        double heatModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleTrayMoveHeight).getDistance();//加热模块托盘z轴移动高度
-        double solutionModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.solutionModuleTrayMoveHeight).getDistance();//加液模块托盘z轴移动高度
-        double capModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.capModuleCapMoveHeight).getDistance();//拍子升降模块拍子z轴移动高度
-
-        Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
-        Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
-//        Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
-        Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
         Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
 
         return runAsync(() -> {
             try {
-                if(trayState != null){
-                    trayState.setHeatModuleId(heatModuleCode);
-                }
                 capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand());//提升拍子存放区至拍子夹取的高度
-                gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，准备夹取托盘
-                gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧，夹住托盘
-                deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块
-                gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
-                deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(0);//加液模块是否有托盘
-//                gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
-                gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
-                deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);Thread.sleep(3200);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块
-                gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使托盘落入加热模块
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，释放托盘
-                deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(1);//加热模块是否存在托盘
-                if(trayState != null){
-                    trayState.setInHeatModule(true);
-                }
-                gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
-                gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
-//                deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，准备夹取拍子
-                gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹爪落入拍子升降模块拍子孔位
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧，夹住拍子
-                gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
-                gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方
-                capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区
-//                deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);Thread.sleep(2500);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块
-                gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹拍子落入加热模块
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，释放拍子
-                deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(true);//加热模块是否存在拍子
-                gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
+                actionCommandService.moveTrayToHeatModule(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//将托盘移动至指定加热模块
+                actionCommandService.heatModuleInstallCap(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//指定加热模块安装拍子
                 gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
             } finally {
-                deviceStateService.getDeviceState().getGantryArm().setIdle(true);
-                deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false);
+                deviceStateService.getDeviceState().getGantryArm().setIdle(true);//机械臂空闲
+                deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false);//该指令结束
                 solutionModuleService.releaseSolutionModule();//释放加液区等待
             }
         });

src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java

@@ -6,11 +6,10 @@ import com.iflytop.gd.app.model.bo.status.device.SolutionModuleState;
 import com.iflytop.gd.app.model.bo.status.device.TrayState;
 import com.iflytop.gd.app.model.dto.CmdDTO;
 import com.iflytop.gd.app.service.api.DevicePositionService;
-import com.iflytop.gd.app.service.device.DeviceCommandTempUtilService;
+import com.iflytop.gd.app.service.device.ActionCommandService;
 import com.iflytop.gd.app.service.device.DeviceStateService;
 import com.iflytop.gd.app.service.device.module.CapModuleService;
 import com.iflytop.gd.app.service.device.module.GantryModuleService;
-import com.iflytop.gd.app.service.device.module.HeatModuleService;
 import com.iflytop.gd.app.service.device.module.SolutionModuleService;
 import com.iflytop.gd.common.annotation.CommandMapping;
 import com.iflytop.gd.common.enums.HeatModuleCode;
@@ -32,13 +31,12 @@ import java.util.concurrent.CompletableFuture;
 @RequiredArgsConstructor
 @CommandMapping("move_to_solution_area")//业务指令注解
 public class MoveToSolutionAreaCommand extends BaseCommandHandler {
-    private final HeatModuleService heatModuleService;
-    private final CapModuleService capModuleService;
     private final SolutionModuleService solutionModuleService;
     private final GantryModuleService gantryModuleService;
+    private final CapModuleService capModuleService;
     private final DevicePositionService devicePositionService;
     private final DeviceStateService deviceStateService;
-    private final DeviceCommandTempUtilService deviceCommandTempUtilService;
+    private final ActionCommandService actionCommandService;
 
     @Override
     public CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception {
@@ -69,12 +67,12 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
 //                throw new AppException(ResultCode.TARGET_HEAT_MODULE_NO_TRAY);
 //            }
             SolutionModuleState solutionModuleState = deviceStateService.getDeviceState().getSolutionModule();
-            if(solutionModuleState.getTrayStatus() == 1){
+            if (solutionModuleState.getTrayStatus() == 1) {
                 throw new AppException(ResultCode.SOLUTION_MODULE_OCCUPIED);
             }
             if (trayState != null && trayState.getCrafts() != null) {
                 CraftStates craftStates = trayState.getCrafts().getState();
-                if(!(CraftStates.ERROR.equals(craftStates) || CraftStates.STOPPED.equals(craftStates))){
+                if (!(CraftStates.ERROR.equals(craftStates) || CraftStates.STOPPED.equals(craftStates))) {
                     throw new AppException(ResultCode.CRAFT_RUNNING);
                 }
             }
@@ -83,67 +81,18 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
             throw e;
         }
 
-        double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //获取拍子夹取距离
-        double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离
-        double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离
-        double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离
-        double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //托盘升降抬升距离
-        double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
-        double heatModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleCapMoveHeight).getDistance();//加热模块拍子移动高度
-        double heatModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleTrayMoveHeight).getDistance();//加热模块托盘z轴移动高度
-        double solutionModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.solutionModuleTrayMoveHeight).getDistance();//加液模块托盘z轴移动高度
-        double capModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.capModuleCapMoveHeight).getDistance();//拍子升降模块拍子z轴移动高度
-
-        Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
-        Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
-//        Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
-        Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
         Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
 
         return runAsync(() -> {
             try {
                 solutionModuleService.requestSolutionModule();//申请使用加液区并等待
                 capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区至拍子夹取的高度
-                gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
-                deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起
-                heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLift);//抬升指定加热位托盘
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，准备夹取拍子
-                gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧，夹住拍子
-                deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子
-                capModuleService.capMotorMoveByNumNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), -1);//拍子存放模块下降1个拍子位置
-//                deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块
-                gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
-                gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
-//                deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 临时避 恢复抬起状态
-                gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，释放夹取的拍子
-                gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
-
-                gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，准备夹取托盘
-                gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧，夹住托盘
-                deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(0);//加热模块是否存在托盘
-                if(trayState != null){
-                    trayState.setInHeatModule(false);
-                }
-                deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//TODO 临时避让下降
-                gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);Thread.sleep(2500);//抬升z轴
-//                gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
-                gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
-                deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(null, heatModuleCode);//TODO 临时避 恢复抬起状态
-                gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块
-                deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(1);//加液模块是否存在托盘
-                if(trayState != null){
-                    trayState.setInSolutionModule(true);
-                }
-                gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，释放托盘
-                gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
+                actionCommandService.heatModuleRemoveCap(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//指定加热模块移除拍子
+                actionCommandService.heatModuleRemoveTray(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//指定加热模块移除托盘
                 gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
             } finally {
-                deviceStateService.getDeviceState().getGantryArm().setIdle(true);
-                deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false);
+                deviceStateService.getDeviceState().getGantryArm().setIdle(true);//机械臂空闲
+                deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false);//该指令结束
             }
         });
     }

src/main/java/com/iflytop/gd/app/model/bo/status/device/HeatModuleState.java

@@ -21,6 +21,9 @@ public class HeatModuleState {
     @Schema(description = "加热模块code")
     private HeatModuleCode moduleCode;
 
+    @Schema(description = "升降锁定，true的时候不可以进行升降")
+    private boolean liftLock = false;
+
     @Schema(description = "托盘升降状态，0为降下，1为抬起")
     private int trayUp = 0;
 

src/main/java/com/iflytop/gd/app/service/api/TestService.java

@@ -5,12 +5,12 @@ import com.iflytop.gd.app.model.dto.AllSensorDTO;
 import com.iflytop.gd.app.service.device.DeviceCommandService;
 import com.iflytop.gd.app.service.device.DeviceStateService;
 import com.iflytop.gd.app.service.device.module.GantryModuleService;
-import com.iflytop.gd.app.service.device.module.HeatModuleService;
 import com.iflytop.gd.common.command.CommandFuture;
 import com.iflytop.gd.common.command.DeviceCommandBundle;
 import com.iflytop.gd.common.command.DeviceCommandGenerator;
 import com.iflytop.gd.common.enums.HeatModuleCode;
 import com.iflytop.gd.common.enums.HeatingType;
+import com.iflytop.gd.common.enums.data.DevicePositionCode;
 import com.iflytop.gd.common.utils.CommandUtil;
 import com.iflytop.gd.hardware.exception.HardwareException;
 import com.iflytop.gd.hardware.service.GDDeviceStatusService;
@@ -32,7 +32,7 @@ public class TestService {
     private final GDDeviceStatusService gdDeviceStatusService;
     private final DeviceStateService deviceStateService;
     private final DeviceCommandService deviceCommandService;
-    private final HeatModuleService heatModuleService;
+    private final DevicePositionService devicePositionService;
     private final GantryModuleService gantryModuleService;
 
     public List<AllSensorDTO> getAllSensor() throws HardwareException {
@@ -92,21 +92,21 @@ public class TestService {
     }
 
     public void moveManyPoints() throws Exception {
-        Point3D heatArea1TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_01);
-        Point3D heatArea2TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_02);
-        Point3D heatArea3TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_03);
-        Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);
-        Point3D heatArea5TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_05);
-        Point3D heatArea6TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_06);
+        Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();
+        Point3D heatArea1TrayClawPoint3D = devicePositionService.getPosition(DevicePositionCode.heatArea1TrayClawPoint).getPoint3D();
+        Point3D heatArea2TrayClawPoint3D = devicePositionService.getPosition(DevicePositionCode.heatArea2TrayClawPoint).getPoint3D();
+        Point3D heatArea3TrayClawPoint3D = devicePositionService.getPosition(DevicePositionCode.heatArea3TrayClawPoint).getPoint3D();
+        Point3D heatArea4TrayClawPoint3D = devicePositionService.getPosition(DevicePositionCode.heatArea4TrayClawPoint).getPoint3D();
+        Point3D heatArea5TrayClawPoint3D = devicePositionService.getPosition(DevicePositionCode.heatArea5TrayClawPoint).getPoint3D();
+        Point3D heatArea6TrayClawPoint3D = devicePositionService.getPosition(DevicePositionCode.heatArea6TrayClawPoint).getPoint3D();
 
-        gantryModuleService.moveManyPoints(
-                heatArea3TrayClawPoint3D,
-                heatArea2TrayClawPoint3D,
-                heatArea1TrayClawPoint3D,
+        gantryModuleService.moveXYManyPoints(
+                liquidAreaTrayPoint3D,
                 heatArea4TrayClawPoint3D,
-                heatArea5TrayClawPoint3D,
+                heatArea1TrayClawPoint3D,
+                heatArea3TrayClawPoint3D,
                 heatArea6TrayClawPoint3D
-               );
+        );
     }
 
 }

src/main/java/com/iflytop/gd/app/service/device/ActionCommandService.java

@@ -0,0 +1,174 @@
+package com.iflytop.gd.app.service.device;
+
+import com.iflytop.gd.app.model.bo.Point3D;
+import com.iflytop.gd.app.model.bo.status.device.TrayState;
+import com.iflytop.gd.app.service.api.DevicePositionService;
+import com.iflytop.gd.app.service.device.module.CapModuleService;
+import com.iflytop.gd.app.service.device.module.GantryModuleService;
+import com.iflytop.gd.app.service.device.module.HeatModuleService;
+import com.iflytop.gd.common.enums.ClawItemType;
+import com.iflytop.gd.common.enums.HeatModuleCode;
+import com.iflytop.gd.common.enums.data.DevicePositionCode;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Service;
+
+/**
+ * 动作指令
+ */
+@Slf4j
+@Service
+@RequiredArgsConstructor
+public class ActionCommandService {
+    private final DeviceCommandService deviceCommandService;
+    private final DevicePositionService devicePositionService;
+    private final DeviceStateService deviceStateService;
+    private final HeatModuleService heatModuleService;
+    private final CapModuleService capModuleService;
+    private final GantryModuleService gantryModuleService;
+    private final DeviceCommandTempUtilService deviceCommandTempUtilService;
+
+
+    /**
+     * 指定加热模块移除托盘
+     */
+    public void heatModuleRemoveTray(String commandId, String command, HeatModuleCode heatModuleCode) throws Exception {
+        double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离
+        double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离
+        double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
+        double heatModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleTrayMoveHeight).getDistance();//加热模块托盘z轴移动高度
+        double solutionModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.solutionModuleTrayMoveHeight).getDistance();//加液模块托盘z轴移动高度
+        Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
+        Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
+
+        TrayState trayState = deviceStateService.getDeviceState().getTrayStateByHeatModuleCode(heatModuleCode);
+
+        heatModuleService.heaterMotorMoveNoWait(commandId, command, heatModuleCode, trayLower);//确保目标加热模块是抬升状态
+        gantryModuleService.gantryXYMove(commandId, command, heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
+        gantryModuleService.clawMove(commandId, command, clawTrayPick);//将夹爪打开，准备夹取托盘
+        gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
+        gantryModuleService.clawMove(commandId, command, clawTrayGrip);//将夹爪收紧，夹住托盘
+        deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(0);//加热模块是否存在托盘
+        if (trayState != null) {
+            trayState.setInHeatModule(false);
+        }
+        heatModuleService.heaterMotorMoveNoWait(commandId, command, heatModuleCode, trayLower);//下降加热模块
+        gantryModuleService.gantryZMove(commandId, command, 0);//抬升z轴
+        Thread.sleep(2500);
+        deviceCommandTempUtilService.carryMoveXYToSolutionManyPoints(commandId, command, heatModuleCode, ClawItemType.tray);//携带托盘移动至加液模块
+        gantryModuleService.gantryXYMove(commandId, command, liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
+        deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(commandId, command, heatModuleCode);//移动完毕恢复抬升状态
+        gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块
+        deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(1);//加液模块是否存在托盘
+        if (trayState != null) {
+            trayState.setInSolutionModule(true);
+        }
+        gantryModuleService.clawMove(commandId, command, clawTrayPick);//将夹爪打开，释放托盘
+        gantryModuleService.gantryZMove(commandId, command, 0);//抬升z轴
+    }
+
+    /**
+     * 指定加热模块移除拍子
+     */
+    public void heatModuleRemoveCap(String commandId, String command, HeatModuleCode heatModuleCode) throws Exception {
+        double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //获取拍子夹取距离
+        double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离
+        double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
+        double heatModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleCapMoveHeight).getDistance();//加热模块拍子移动高度
+        double capModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.capModuleCapMoveHeight).getDistance();//拍子升降模块拍子z轴移动高度
+
+        Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
+
+        heatModuleService.heaterMotorMoveNoWait(commandId, command, heatModuleCode, trayLower);//确保目标加热模块是抬升状态
+
+        gantryModuleService.gantryXYMove(commandId, command, heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
+        deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起
+        gantryModuleService.clawMove(commandId, command, clawCapPick);//将夹爪打开，准备夹取拍子
+        gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位
+        gantryModuleService.clawMove(commandId, command, clawCapGrip);//将夹爪收紧，夹住拍子
+        deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子
+        capModuleService.capMotorMoveByNumNoWait(commandId, command, -1);//拍子存放模块下降1个拍子位置
+        gantryModuleService.gantryZMove(commandId, command, 0);//抬升z轴
+        heatModuleService.heaterMotorMoveNoWait(commandId, command, heatModuleCode, trayLower);//下降加热模块
+        Thread.sleep(2500);
+        deviceCommandTempUtilService.carryMoveXYToSolutionManyPoints(commandId, command, heatModuleCode, ClawItemType.cap);//携带拍子移动至存放区
+        deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(commandId, command, heatModuleCode);//移动完毕恢复抬升状态
+        gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区
+        gantryModuleService.clawMove(commandId, command, clawCapPick);//将夹爪打开，释放夹取的拍子
+        gantryModuleService.gantryZMove(commandId, command, 0);//抬升z轴
+    }
+
+    /**
+     * 指定加热模块安装拍子
+     */
+    public void heatModuleInstallCap(String commandId, String command, HeatModuleCode heatModuleCode) throws Exception {
+        double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //获取拍子夹取距离
+        double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离
+        double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
+        double heatModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleCapMoveHeight).getDistance();//加热模块拍子移动高度
+        double capModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.capModuleCapMoveHeight).getDistance();//拍子升降模块拍子z轴移动高度
+        Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
+
+        heatModuleService.heaterMotorMoveNoWait(commandId, command, heatModuleCode, trayLower);//确保目标加热模块是下降状态
+
+        gantryModuleService.gantryXYMove(commandId, command, capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
+        deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(0);//将加热模块托盘状态改为下降
+        gantryModuleService.clawMove(commandId, command, clawCapPick);//将夹爪打开，准备夹取拍子
+        gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹爪落入拍子升降模块拍子孔位
+        gantryModuleService.clawMove(commandId, command, clawCapGrip);//将夹爪收紧，夹住拍子
+        gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴
+        deviceCommandTempUtilService.carryMoveXYToHeatManyPoints(commandId, command, heatModuleCode, ClawItemType.cap);//将拍子移动至目标加热模块上方
+        capModuleService.capUpBalanceNoWait(commandId, command); //提升拍子存放区
+
+        deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(commandId, command, heatModuleCode);
+        Thread.sleep(4500);
+
+        gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹拍子落入加热模块
+        gantryModuleService.clawMove(commandId, command, clawCapPick);//将夹爪打开，释放拍子
+        deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(true);//加热模块是否存在拍子
+        gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴
+    }
+
+    /**
+     * 从加液模块移动托盘至指定加热模块
+     */
+    public void moveTrayToHeatModule(String commandId, String command, HeatModuleCode heatModuleCode) throws Exception {
+        double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离
+        double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离
+        double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
+        double heatModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleTrayMoveHeight).getDistance();//加热模块托盘z轴移动高度
+        double solutionModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.solutionModuleTrayMoveHeight).getDistance();//加液模块托盘z轴移动高度
+        Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
+
+        TrayState trayState = deviceStateService.getDeviceState().getTrayInSolutionModule();
+        if (trayState != null) {
+            trayState.setHeatModuleId(heatModuleCode);
+        }
+
+        heatModuleService.heaterMotorMoveNoWait(commandId, command, heatModuleCode, trayLower);//确保目标加热模块是下降状态
+
+        gantryModuleService.gantryXYMove(commandId, command, liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方
+        deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(0);//将加热模块托盘状态改为下降
+        gantryModuleService.clawMove(commandId, command, clawTrayPick);//将夹爪打开，准备夹取托盘
+        gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
+        gantryModuleService.clawMove(commandId, command, clawTrayGrip);//将夹爪收紧，夹住托盘
+        gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴
+        if (trayState != null) {
+            trayState.setInSolutionModule(false);
+        }
+        deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(0);//加液模块是否有托盘
+        deviceCommandTempUtilService.carryMoveXYToHeatManyPoints(commandId, command, heatModuleCode, ClawItemType.tray);//将托盘移动至目标加热模块上方
+
+        deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(commandId, command, heatModuleCode);//抬升避让的加热模块
+        Thread.sleep(3200);
+
+        gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使托盘落入加热模块
+        gantryModuleService.clawMove(commandId, command, clawTrayPick);//将夹爪打开，释放托盘
+        deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(1);//加热模块是否存在托盘
+        if (trayState != null) {
+            trayState.setInHeatModule(true);
+        }
+        gantryModuleService.gantryZMove(commandId, command, 0);//抬升z轴
+    }
+
+}

src/main/java/com/iflytop/gd/app/service/device/DeviceCommandTempUtilService.java

@@ -1,11 +1,15 @@
 package com.iflytop.gd.app.service.device;
 
+import com.iflytop.gd.app.model.bo.Point3D;
 import com.iflytop.gd.app.model.bo.status.device.HeatModuleState;
 import com.iflytop.gd.app.service.api.DevicePositionService;
+import com.iflytop.gd.app.service.device.module.GantryModuleService;
 import com.iflytop.gd.app.service.device.module.HeatModuleService;
+import com.iflytop.gd.app.service.device.module.SolutionModuleService;
 import com.iflytop.gd.common.command.CommandFuture;
 import com.iflytop.gd.common.command.DeviceCommandBundle;
 import com.iflytop.gd.common.command.DeviceCommandGenerator;
+import com.iflytop.gd.common.enums.ClawItemType;
 import com.iflytop.gd.common.enums.HeatModuleCode;
 import com.iflytop.gd.common.enums.data.DevicePositionCode;
 import com.iflytop.gd.common.utils.CommandUtil;
@@ -17,7 +21,6 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collections;
 import java.util.List;
-import java.util.concurrent.CompletableFuture;
 
 /**
  * 临时方法 以后应该会废弃
@@ -28,11 +31,339 @@ import java.util.concurrent.CompletableFuture;
 public class DeviceCommandTempUtilService {
     private final DeviceCommandService deviceCommandService;
     private final HeatModuleService heatModuleService;
+    private final GantryModuleService gantryModuleService;
     private final DevicePositionService devicePositionService;
     private final DeviceStateService deviceStateService;
 
     private final List<HeatModuleCode> moveTrayHeatModuleAvoidUpStateList = Collections.synchronizedList(new ArrayList<>());
 
+
+    /**
+     * 携带托盘或者拍子多点依次移动至加液模块或拍子存放区
+     */
+    public void carryMoveXYToSolutionManyPoints(HeatModuleCode targetHeatModuleCode, ClawItemType clawItemType) throws Exception {
+        carryMoveXYToSolutionManyPoints(null, null, targetHeatModuleCode, clawItemType);
+    }
+
+
+    /**
+     * 携带托盘或者拍子多点依次移动至加液模块或拍子存放区
+     */
+    public void carryMoveXYToSolutionManyPoints(String commandId, String command, HeatModuleCode targetHeatModuleCode, ClawItemType clawItemType) throws Exception {
+        Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
+        Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
+        Point3D heatArea1TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_01);
+        Point3D heatArea3TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_03);
+        Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);
+
+        Point3D heatArea3CapClawPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(HeatModuleCode.heat_module_03);
+
+        Point3D[] points = new Point3D[]{};
+        if (HeatModuleCode.heat_module_01.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 存放区
+                points = new Point3D[]{capStorageCapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_01,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 4、加液
+                points = new Point3D[]{heatArea4TrayClawPoint3D, liquidAreaTrayPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_01,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_02.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 存放区
+                points = new Point3D[]{capStorageCapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 1、4、加液
+                points = new Point3D[]{heatArea1TrayClawPoint3D, heatArea4TrayClawPoint3D, liquidAreaTrayPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_03.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 存放区
+                points = new Point3D[]{capStorageCapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 1、4、加液
+                points = new Point3D[]{heatArea1TrayClawPoint3D, heatArea4TrayClawPoint3D, liquidAreaTrayPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_04.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 存放区
+                points = new Point3D[]{capStorageCapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 加液
+                points = new Point3D[]{liquidAreaTrayPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_01,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_05.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 存放区
+                points = new Point3D[]{capStorageCapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 加液
+                points = new Point3D[]{liquidAreaTrayPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_01,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_06.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 3、存放区
+                points = new Point3D[]{heatArea3CapClawPoint3D, capStorageCapClawPoint3D};
+                //不避让模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                if(deviceStateService.getDeviceState().getHeatModuleByCode(HeatModuleCode.heat_module_05).getTrayUp() == 1){
+                    //路线 3、1、4、加液
+                    points = new Point3D[]{heatArea3TrayClawPoint3D, heatArea1TrayClawPoint3D, heatArea4TrayClawPoint3D, liquidAreaTrayPoint3D};
+                    //不避让的模块
+                    moveTrayHeatModuleAvoidDown(commandId, command, null,
+                            HeatModuleCode.heat_module_05,
+                            HeatModuleCode.heat_module_06);
+                }else{
+                    //路线 加液
+                    points = new Point3D[]{liquidAreaTrayPoint3D};
+                    //不避让的模块
+                    moveTrayHeatModuleAvoidDown(commandId, command, null,
+                            HeatModuleCode.heat_module_01,
+                            HeatModuleCode.heat_module_02,
+                            HeatModuleCode.heat_module_03,
+                            HeatModuleCode.heat_module_06);
+                }
+
+            }
+        }
+        gantryModuleService.moveXYManyPoints(points);
+    }
+
+    /**
+     * 携带托盘或者拍子多点依次移动至加热模块
+     */
+    public void carryMoveXYToHeatManyPoints(HeatModuleCode targetHeatModuleCode, ClawItemType clawItemType) throws Exception {
+        carryMoveXYToHeatManyPoints(null, null, targetHeatModuleCode, clawItemType);
+    }
+
+
+    /**
+     * 携带托盘或者拍子多点依次移动至加热模块
+     */
+    public void carryMoveXYToHeatManyPoints(String commandId, String command, HeatModuleCode targetHeatModuleCode, ClawItemType clawItemType) throws Exception {
+        Point3D heatArea1TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_01);
+        Point3D heatArea2TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_02);
+        Point3D heatArea3TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_03);
+        Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);
+        Point3D heatArea5TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_05);
+        Point3D heatArea6TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_06);
+
+        Point3D heatArea1CapClawPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(HeatModuleCode.heat_module_01);
+        Point3D heatArea2CapClawPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(HeatModuleCode.heat_module_02);
+        Point3D heatArea3CapClawPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(HeatModuleCode.heat_module_03);
+        Point3D heatArea4CapClawPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(HeatModuleCode.heat_module_04);
+        Point3D heatArea5CapClawPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(HeatModuleCode.heat_module_05);
+        Point3D heatArea6CapClawPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(HeatModuleCode.heat_module_06);
+
+        Point3D[] points = new Point3D[]{};
+        if (HeatModuleCode.heat_module_01.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 1
+                points = new Point3D[]{heatArea1CapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_01,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 4、1
+                points = new Point3D[]{heatArea4TrayClawPoint3D, heatArea1TrayClawPoint3D, heatArea2TrayClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_01,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_02.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 2
+                points = new Point3D[]{heatArea2CapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 4、1、2
+                points = new Point3D[]{heatArea4TrayClawPoint3D, heatArea1TrayClawPoint3D, heatArea2TrayClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_03.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 3
+                points = new Point3D[]{heatArea3CapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 4、1、3
+                points = new Point3D[]{heatArea4TrayClawPoint3D, heatArea1TrayClawPoint3D, heatArea3TrayClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_04.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 4
+                points = new Point3D[]{heatArea4CapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 4
+                points = new Point3D[]{heatArea4TrayClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_01,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_05.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 5
+                points = new Point3D[]{heatArea5CapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                //路线 5
+                points = new Point3D[]{heatArea5TrayClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_01,
+                        HeatModuleCode.heat_module_02,
+                        HeatModuleCode.heat_module_03,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            }
+        } else if (HeatModuleCode.heat_module_06.equals(targetHeatModuleCode)) {
+            if (ClawItemType.cap.equals(clawItemType)) {//移动拍子
+                //路线 3、6
+                points = new Point3D[]{heatArea3CapClawPoint3D, heatArea6CapClawPoint3D};
+                //不避让的模块
+                moveTrayHeatModuleAvoidDown(commandId, command, null,
+                        HeatModuleCode.heat_module_04,
+                        HeatModuleCode.heat_module_05,
+                        HeatModuleCode.heat_module_06);
+            } else if (ClawItemType.tray.equals(clawItemType)) {//移动托盘
+                if(deviceStateService.getDeviceState().getHeatModuleByCode(HeatModuleCode.heat_module_05).getTrayUp() == 1){
+                    //路线 4、1、3、6
+                    points = new Point3D[]{heatArea4TrayClawPoint3D, heatArea1TrayClawPoint3D, heatArea3TrayClawPoint3D, heatArea6TrayClawPoint3D};
+                    //不避让的模块
+                    moveTrayHeatModuleAvoidDown(commandId, command, null,
+                            HeatModuleCode.heat_module_05,
+                            HeatModuleCode.heat_module_06);
+                }else{
+                    //路线 6
+                    points = new Point3D[]{heatArea6TrayClawPoint3D};
+                    //不避让的模块
+                    moveTrayHeatModuleAvoidDown(commandId, command, null,
+                            HeatModuleCode.heat_module_01,
+                            HeatModuleCode.heat_module_02,
+                            HeatModuleCode.heat_module_03,
+                            HeatModuleCode.heat_module_06);
+                }
+
+            }
+        }
+        gantryModuleService.moveXYManyPoints(points);
+    }
+
     /**
      * 避让抬升
      * 临时移动托盘的时候，加热模块升降避让
@@ -50,6 +381,7 @@ public class DeviceCommandTempUtilService {
     public void moveTrayHeatModuleAvoidUp(HeatModuleCode targetHeatModuleCode, HeatModuleCode... exceptionHeatModuleCodes) throws Exception {
         moveTrayHeatModuleAvoidUp(null, null, targetHeatModuleCode, exceptionHeatModuleCodes);
     }
+
     /**
      * 避让抬升
      * 临时移动托盘的时候，加热模块升降避让
@@ -64,7 +396,6 @@ public class DeviceCommandTempUtilService {
             }
         }
         if (targetHeatModuleCode != null) {
-
             DeviceCommandBundle deviceCommand = getHeaterMotorMoveDeviceCommand(targetHeatModuleCode, trayLift);
             deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
         }

src/main/java/com/iflytop/gd/app/service/device/module/GantryModuleService.java

@@ -1,12 +1,15 @@
 package com.iflytop.gd.app.service.device.module;
 
 import com.iflytop.gd.app.model.bo.Point3D;
+import com.iflytop.gd.app.model.bo.status.device.TrayState;
 import com.iflytop.gd.app.service.api.DevicePositionService;
 import com.iflytop.gd.app.service.device.DeviceCommandService;
+import com.iflytop.gd.app.service.device.DeviceCommandTempUtilService;
 import com.iflytop.gd.app.service.device.DeviceStateService;
 import com.iflytop.gd.common.command.CommandFuture;
 import com.iflytop.gd.common.command.DeviceCommandBundle;
 import com.iflytop.gd.common.command.DeviceCommandGenerator;
+import com.iflytop.gd.common.enums.ClawItemType;
 import com.iflytop.gd.common.enums.HeatModuleCode;
 import com.iflytop.gd.common.enums.data.DevicePositionCode;
 import com.iflytop.gd.common.utils.CommandUtil;
@@ -28,25 +31,39 @@ public class GantryModuleService {
     private final DevicePositionService devicePositionService;
     private final DeviceStateService deviceStateService;
 
-
     /**
      * 多点依次移动
      */
-    public void moveManyPoints(Point3D... point3DS) throws Exception {
-        moveManyPoints(null, null, point3DS);
+    public void moveXYManyPoints(Point3D... point3DS) throws Exception {
+        moveXYManyPoints(null, null, point3DS);
     }
 
 
     /**
      * 多点依次移动
      */
-    public void moveManyPoints(String commandId, String command, Point3D... point3DS) throws Exception {
+    public void moveXYManyPoints(String commandId, String command, Point3D... point3DS) throws Exception {
         for (Point3D point3D : point3DS) {
             gantryXYMove(commandId, command, point3D);
         }
     }
 
     /**
+     * 龙门架机械臂Z移动到0点
+     */
+    public void gantryZMoveZero() throws Exception {
+        gantryZMoveZero(null, null);
+    }
+
+    /**
+     * 龙门架机械臂Z移动到0点
+     */
+    public void gantryZMoveZero(String commandId, String command) throws Exception {
+        gantryZMove(commandId, command, 0.0);
+    }
+
+
+    /**
      * 龙门架机械臂移动到0点
      */
     public void gantryMoveZero() throws Exception {

src/main/java/com/iflytop/gd/app/service/device/module/HeatModuleService.java

@@ -62,6 +62,28 @@ public class HeatModuleService {
     /**
      * 加热模块升降电机移动到指定位置
      */
+    public void heaterMotorMoveNoWait(HeatModuleCode heatModuleId, double position) throws Exception {
+        heaterMotorMoveNoWait(null, null, heatModuleId, position);
+    }
+
+    /**
+     * 加热模块升降电机移动到指定位置
+     */
+    public void heaterMotorMoveNoWait(String commandId, String command, HeatModuleCode heatModuleId, double position) throws Exception {
+        DeviceCommandBundle deviceCommand = switch (heatModuleId) {
+            case heat_module_01 -> DeviceCommandGenerator.heaterMotor1Move(position);
+            case heat_module_02 -> DeviceCommandGenerator.heaterMotor2Move(position);
+            case heat_module_03 -> DeviceCommandGenerator.heaterMotor3Move(position);
+            case heat_module_04 -> DeviceCommandGenerator.heaterMotor4Move(position);
+            case heat_module_05 -> DeviceCommandGenerator.heaterMotor5Move(position);
+            case heat_module_06 -> DeviceCommandGenerator.heaterMotor6Move(position);
+        };
+        deviceCommandService.sendCommand(commandId, command, deviceCommand);
+    }
+
+    /**
+     * 加热模块升降电机移动到指定位置
+     */
     public void heaterMotorMove(HeatModuleCode heatModuleId, double position) throws Exception {
         heaterMotorMove(null, null, heatModuleId, position);
     }

src/main/java/com/iflytop/gd/common/enums/ClawItemType.java

@@ -0,0 +1,13 @@
+package com.iflytop.gd.common.enums;
+
+import lombok.Getter;
+
+/**
+ * 夹爪夹取的物品类型
+ */
+@Getter
+public enum ClawItemType {
+    tray,
+    cap,
+    tube
+}