From b4ab15d5df42c69af6c5b56650fef75fcdf52e63 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=99=BD=E5=87=A4=E5=90=89?= Date: Sat, 14 Jun 2025 19:59:42 +0800 Subject: [PATCH] =?UTF-8?q?feat:=E7=A7=BB=E5=8A=A8=E6=97=B6=E8=B7=AF?= =?UTF-8?q?=E7=BA=BF=E9=81=BF=E9=9A=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../app/command/control/MoveToHeatAreaCommand.java | 63 +--- .../command/control/MoveToSolutionAreaCommand.java | 69 +---- .../model/bo/status/device/HeatModuleState.java | 3 + .../iflytop/gd/app/service/api/TestService.java | 28 +- .../app/service/device/ActionCommandService.java | 174 +++++++++++ .../device/DeviceCommandTempUtilService.java | 335 ++++++++++++++++++++- .../service/device/module/GantryModuleService.java | 25 +- .../service/device/module/HeatModuleService.java | 22 ++ .../com/iflytop/gd/common/enums/ClawItemType.java | 13 + 9 files changed, 598 insertions(+), 134 deletions(-) create mode 100644 src/main/java/com/iflytop/gd/app/service/device/ActionCommandService.java create mode 100644 src/main/java/com/iflytop/gd/common/enums/ClawItemType.java diff --git a/src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java b/src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java index d905ee9..07f6d63 100644 --- a/src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java +++ b/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 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();//释放加液区等待 } }); diff --git a/src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java b/src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java index f583419..1d8a847 100644 --- a/src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java +++ b/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 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);//该指令结束 } }); } diff --git a/src/main/java/com/iflytop/gd/app/model/bo/status/device/HeatModuleState.java b/src/main/java/com/iflytop/gd/app/model/bo/status/device/HeatModuleState.java index bb4a98e..7849c79 100644 --- a/src/main/java/com/iflytop/gd/app/model/bo/status/device/HeatModuleState.java +++ b/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; diff --git a/src/main/java/com/iflytop/gd/app/service/api/TestService.java b/src/main/java/com/iflytop/gd/app/service/api/TestService.java index e5d6397..904f81e 100644 --- a/src/main/java/com/iflytop/gd/app/service/api/TestService.java +++ b/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 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 - ); + ); } } diff --git a/src/main/java/com/iflytop/gd/app/service/device/ActionCommandService.java b/src/main/java/com/iflytop/gd/app/service/device/ActionCommandService.java new file mode 100644 index 0000000..58b50a6 --- /dev/null +++ b/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轴 + } + +} diff --git a/src/main/java/com/iflytop/gd/app/service/device/DeviceCommandTempUtilService.java b/src/main/java/com/iflytop/gd/app/service/device/DeviceCommandTempUtilService.java index 5067273..d6b459c 100644 --- a/src/main/java/com/iflytop/gd/app/service/device/DeviceCommandTempUtilService.java +++ b/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 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); } diff --git a/src/main/java/com/iflytop/gd/app/service/device/module/GantryModuleService.java b/src/main/java/com/iflytop/gd/app/service/device/module/GantryModuleService.java index 5275dfb..2a397d3 100644 --- a/src/main/java/com/iflytop/gd/app/service/device/module/GantryModuleService.java +++ b/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 { diff --git a/src/main/java/com/iflytop/gd/app/service/device/module/HeatModuleService.java b/src/main/java/com/iflytop/gd/app/service/device/module/HeatModuleService.java index 6c8a8f8..fcfec0a 100644 --- a/src/main/java/com/iflytop/gd/app/service/device/module/HeatModuleService.java +++ b/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); } diff --git a/src/main/java/com/iflytop/gd/common/enums/ClawItemType.java b/src/main/java/com/iflytop/gd/common/enums/ClawItemType.java new file mode 100644 index 0000000..02eac39 --- /dev/null +++ b/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 +}