From bf8117e5726468883553197c6d6839abb99407e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=99=BD=E5=87=A4=E5=90=89?= Date: Wed, 14 May 2025 21:17:43 +0800 Subject: [PATCH] =?UTF-8?q?feat:=E5=A2=9E=E5=8A=A0=E7=A7=BB=E8=87=B3?= =?UTF-8?q?=E5=8A=A0=E7=83=AD=E3=80=81=E5=8A=A0=E6=B6=B2=E6=A0=A1=E9=AA=8C?= =?UTF-8?q?=E6=89=98=E7=9B=98=E7=8A=B6=E6=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../iflytop/gd/app/cmd/MoveToHeatAreaCommand.java | 112 +++++++++++--------- .../gd/app/cmd/MoveToSolutionAreaCommand.java | 114 ++++++++++++--------- .../gd/app/service/DeviceCommandUtilService.java | 16 +++ .../com/iflytop/gd/common/cmd/CommandHandler.java | 2 +- .../com/iflytop/gd/common/result/ResultCode.java | 7 +- 5 files changed, 155 insertions(+), 96 deletions(-) diff --git a/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java b/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java index 91157d3..4b1ec35 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java @@ -11,11 +11,16 @@ import com.iflytop.gd.app.service.GantryArmService; import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.common.enums.data.DevicePositionCode; +import com.iflytop.gd.common.exception.AppException; +import com.iflytop.gd.common.result.ResultCode; +import com.iflytop.gd.hardware.service.GDDeviceStatusService; +import com.iflytop.gd.hardware.type.IO.InputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; import java.util.concurrent.CompletableFuture; +import java.util.concurrent.atomic.AtomicBoolean; /** * 移至加热 @@ -29,59 +34,72 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { private final DevicePositionService devicePositionService; private final GantryArmService gantryArmService; private final DeviceStateService deviceStateService; + private final AtomicBoolean isExecuting = new AtomicBoolean(false); @Override - public CompletableFuture handle(CmdDTO cmdDTO) { + public CompletableFuture handle(CmdDTO cmdDTO) throws Exception { + if (isExecuting.get()) { + throw new AppException(ResultCode.COMMAND_ALREADY_EXECUTING); + } + isExecuting.set(true); String heatId = cmdDTO.getStringParam("heatId"); HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId); - //TODO 需要校验目标加热位是否有托盘，加液位置是否有托盘 - //TODO 有的指令不能并行，前端也应该友好提示，机械臂是否繁忙 - //TODO 简单的前端确认弹窗是否有托盘即可 + //校验目标加热位是否有托盘 + try{ + Boolean heatModuleTray = deviceCommandUtilService.heatModuleTray(heatModuleId); + if (!heatModuleTray) { + throw new AppException(ResultCode.TARGET_HEAT_MODULE_NO_TRAY); + } + }finally { + isExecuting.set(false); + } return runAsync(() -> { - TrayState trayState = deviceStateService.getTrayInSolutionModule(); + try { + TrayState trayState = deviceStateService.getTrayInSolutionModule(); - Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液区托盘夹爪点 - deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将机械臂移动至加液区托盘上方 - double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪张开 - double clawDescend = devicePositionService.getPosition(DevicePositionCode.clawDescend).getDistance();//获取下降机械臂使夹爪可以夹住的距离 - deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住托盘 - double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧 - double traySafetyHeight = devicePositionService.getPosition(DevicePositionCode.traySafetyHeight).getDistance();//获取移动托盘的安全高度 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -traySafetyHeight);//机械臂提升至移动托盘的安全高度 - deviceStateService.setSolutionModuleStateTrayStatus(0); - Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(heatModuleId);//获取指定托盘上方点位 - Point3D heatAreaTrayClawSafetyHeightPoint3D = new Point3D(heatAreaTrayClawPoint3D.getX(), - heatAreaTrayClawPoint3D.getY(), heatAreaTrayClawPoint3D.getZ() - traySafetyHeight);//加热区托盘点位上方减去移动托盘的安全高度 - deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawSafetyHeightPoint3D);//将携带托盘的机械臂移动至托盘上方 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight);//下降机械臂将托盘与试管落入加热区 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//将机械臂提升至托盘上方高度 - Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位; - deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 - double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //拍子夹取距离 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开 - deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住拍子 - double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧 - double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -capSafetyHeight);//机械臂提升至移动拍子的安全高度 - deviceCommandUtilService.capUpBalance(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区 - Point3D heatAreaCapClawPoint3D = deviceCommandUtilService.getHeatAreaCapClawPointPoint3D(heatModuleId);//获取托盘上方拍子点位 - Point3D heatAreaCapClawSafetyHeightPoint3D = new Point3D(heatAreaCapClawPoint3D.getX(), heatAreaCapClawPoint3D.getY(), - heatAreaCapClawPoint3D.getZ() - capSafetyHeight);//加热区拍子上方加上移动拍子的安全高度 - deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawSafetyHeightPoint3D);//将机械臂移动至拍子上方加上移动拍子的安全高度 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight); //降下机械臂使拍子盖住托盘试管 - trayState.setHeatModuleId(heatModuleId); - trayState.setInHeatModule(true); - deviceStateService.setHeatModuleStateCapExist(heatModuleId, true); - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//抬升机械臂至托盘上方 - gantryArmService.setLiquidIdleTrue();//释放加液区等待 - deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 2); - double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置 - deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLower);//下降加热位托盘 - deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 1); + Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液区托盘夹爪点 + deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将机械臂移动至加液区托盘上方 + double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪张开 + double clawDescend = devicePositionService.getPosition(DevicePositionCode.clawDescend).getDistance();//获取下降机械臂使夹爪可以夹住的距离 + deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住托盘 + double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧 + double traySafetyHeight = devicePositionService.getPosition(DevicePositionCode.traySafetyHeight).getDistance();//获取移动托盘的安全高度 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -traySafetyHeight);//机械臂提升至移动托盘的安全高度 + deviceStateService.setSolutionModuleStateTrayStatus(0); + Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(heatModuleId);//获取指定托盘上方点位 + Point3D heatAreaTrayClawSafetyHeightPoint3D = new Point3D(heatAreaTrayClawPoint3D.getX(), heatAreaTrayClawPoint3D.getY(), heatAreaTrayClawPoint3D.getZ() - traySafetyHeight);//加热区托盘点位上方减去移动托盘的安全高度 + deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawSafetyHeightPoint3D);//将携带托盘的机械臂移动至托盘上方 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight);//下降机械臂将托盘与试管落入加热区 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//将机械臂提升至托盘上方高度 + Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位; + deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //拍子夹取距离 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开 + deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住拍子 + double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧 + double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -capSafetyHeight);//机械臂提升至移动拍子的安全高度 + deviceCommandUtilService.capUpBalance(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区 + Point3D heatAreaCapClawPoint3D = deviceCommandUtilService.getHeatAreaCapClawPointPoint3D(heatModuleId);//获取托盘上方拍子点位 + Point3D heatAreaCapClawSafetyHeightPoint3D = new Point3D(heatAreaCapClawPoint3D.getX(), heatAreaCapClawPoint3D.getY(), heatAreaCapClawPoint3D.getZ() - capSafetyHeight);//加热区拍子上方加上移动拍子的安全高度 + deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawSafetyHeightPoint3D);//将机械臂移动至拍子上方加上移动拍子的安全高度 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight); //降下机械臂使拍子盖住托盘试管 + trayState.setHeatModuleId(heatModuleId); + trayState.setInHeatModule(true); + deviceStateService.setHeatModuleStateCapExist(heatModuleId, true); + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//抬升机械臂至托盘上方 + gantryArmService.setLiquidIdleTrue();//释放加液区等待 + deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 2); + double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置 + deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLower);//下降加热位托盘 + deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 1); + } finally { + isExecuting.set(false); + } }); } } diff --git a/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java b/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java index f777acc..bc6c688 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java @@ -11,11 +11,15 @@ import com.iflytop.gd.app.service.GantryArmService; import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.common.enums.data.DevicePositionCode; +import com.iflytop.gd.common.exception.AppException; +import com.iflytop.gd.common.result.ResultCode; +import com.iflytop.gd.hardware.exception.HardwareException; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; import java.util.concurrent.CompletableFuture; +import java.util.concurrent.atomic.AtomicBoolean; /** * 移至加液 @@ -29,59 +33,75 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler { private final DevicePositionService devicePositionService; private final GantryArmService gantryArmService; private final DeviceStateService deviceStateService; + private final AtomicBoolean isExecuting = new AtomicBoolean(false); @Override - public CompletableFuture handle(CmdDTO cmdDTO) { + public CompletableFuture handle(CmdDTO cmdDTO) throws Exception { + if (isExecuting.get()) { + throw new AppException(ResultCode.COMMAND_ALREADY_EXECUTING); + } + isExecuting.set(true); String heatId = cmdDTO.getStringParam("heatId"); HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId); + //校验目标加热位是否有托盘 + try{ + Boolean heatModuleTray = deviceCommandUtilService.heatModuleTray(heatModuleId); + if (heatModuleTray) { + throw new AppException(ResultCode.TARGET_HEAT_MODULE_OCCUPIED); + } + }finally { + isExecuting.set(false); + } return runAsync(() -> { - TrayState trayState = deviceStateService.getTrayInSolutionModule(); + try{ + TrayState trayState = deviceStateService.getTrayInSolutionModule(); - gantryArmService.waitLiquidIdle();//等待加液区空闲 - gantryArmService.setLiquidIdleFalse();//锁定加液区 - double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //获取加热位抬升托盘位置 - deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLift);//抬升加热位托盘 - deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 2); - //TODO 判断托盘是否有拍子 - Point3D heatAreaCapClawPoint3D = deviceCommandUtilService.getHeatAreaCapClawPointPoint3D(heatModuleId);//获取托盘上方拍子点位 - deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPoint3D);//将机械臂移动至拍子上方 - double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //拍子夹取距离 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开 - double clawDescend = devicePositionService.getPosition(DevicePositionCode.clawDescend).getDistance();//获取下降机械臂使夹爪可以夹住的距离 - deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住拍子 - double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧 - double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -capSafetyHeight);//机械臂提升至移动拍子的安全高度 - deviceStateService.setHeatModuleStateCapExist(heatModuleId, false); - deviceCommandUtilService.capMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), -1); //下降拍子存放区1个位置 - Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位; - Point3D capStorageCapClawSafetyHeightPoint3D = new Point3D(capStorageCapClawPoint3D.getX(), capStorageCapClawPoint3D.getY(), - capStorageCapClawPoint3D.getZ() - capSafetyHeight);//拍子存放区上方加上移动拍子的安全高度 - deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawSafetyHeightPoint3D);//移动机械臂至拍子存放区上方上减去拍子的安全高度 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), capSafetyHeight);//下降机械臂，使拍子落入存放区 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick); //松开夹爪，放开拍子 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend);//将机械臂抬升至托盘上方 - Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(heatModuleId);//获取指定托盘上方点位 - deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至托盘上方 - double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪张开 - deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住托盘 - double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧 - double traySafetyHeight = devicePositionService.getPosition(DevicePositionCode.traySafetyHeight).getDistance();//获取移动托盘的安全高度 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -traySafetyHeight);//机械臂提升至移动托盘的安全高度 - deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 0); - trayState.setInHeatModule(false); - Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液区托盘夹爪点 - Point3D liquidAreaTraySafetyHeightPoint3D = new Point3D(liquidAreaTrayPoint3D.getX(), liquidAreaTrayPoint3D.getY(), - liquidAreaTrayPoint3D.getZ() - traySafetyHeight);//加液区托盘点位上方加上移动托盘的安全高度 - deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTraySafetyHeightPoint3D);//移动机械臂至加液区托盘点位上方减去移动托盘的安全高度 - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight);//下降机械臂将托盘与试管落入加液区 - deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//松开夹爪，放下托盘 - deviceStateService.setSolutionModuleStateTrayStatus(1); - trayState.setInSolutionModule(true); - deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//抬升机械臂至托盘上方 + gantryArmService.waitLiquidIdle();//等待加液区空闲 + gantryArmService.setLiquidIdleFalse();//锁定加液区 + double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //获取加热位抬升托盘位置 + deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLift);//抬升加热位托盘 + deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 2); + //TODO 判断托盘是否有拍子 + Point3D heatAreaCapClawPoint3D = deviceCommandUtilService.getHeatAreaCapClawPointPoint3D(heatModuleId);//获取托盘上方拍子点位 + deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPoint3D);//将机械臂移动至拍子上方 + double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //拍子夹取距离 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开 + double clawDescend = devicePositionService.getPosition(DevicePositionCode.clawDescend).getDistance();//获取下降机械臂使夹爪可以夹住的距离 + deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住拍子 + double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧 + double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -capSafetyHeight);//机械臂提升至移动拍子的安全高度 + deviceStateService.setHeatModuleStateCapExist(heatModuleId, false); + deviceCommandUtilService.capMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), -1); //下降拍子存放区1个位置 + Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位; + Point3D capStorageCapClawSafetyHeightPoint3D = new Point3D(capStorageCapClawPoint3D.getX(), capStorageCapClawPoint3D.getY(),capStorageCapClawPoint3D.getZ() - capSafetyHeight);//拍子存放区上方加上移动拍子的安全高度 + deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawSafetyHeightPoint3D);//移动机械臂至拍子存放区上方上减去拍子的安全高度 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), capSafetyHeight);//下降机械臂，使拍子落入存放区 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick); //松开夹爪，放开拍子 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend);//将机械臂抬升至托盘上方 + Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(heatModuleId);//获取指定托盘上方点位 + deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至托盘上方 + double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪张开 + deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住托盘 + double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧 + double traySafetyHeight = devicePositionService.getPosition(DevicePositionCode.traySafetyHeight).getDistance();//获取移动托盘的安全高度 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -traySafetyHeight);//机械臂提升至移动托盘的安全高度 + deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 0); + trayState.setInHeatModule(false); + Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液区托盘夹爪点 + Point3D liquidAreaTraySafetyHeightPoint3D = new Point3D(liquidAreaTrayPoint3D.getX(), liquidAreaTrayPoint3D.getY(),liquidAreaTrayPoint3D.getZ() - traySafetyHeight);//加液区托盘点位上方加上移动托盘的安全高度 + deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTraySafetyHeightPoint3D);//移动机械臂至加液区托盘点位上方减去移动托盘的安全高度 + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight);//下降机械臂将托盘与试管落入加液区 + deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//松开夹爪，放下托盘 + deviceStateService.setSolutionModuleStateTrayStatus(1); + trayState.setInSolutionModule(true); + deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//抬升机械臂至托盘上方 + }finally { + isExecuting.set(false); + } }); } } diff --git a/src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java b/src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java index a9b2ab9..848043a 100644 --- a/src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java +++ b/src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java @@ -10,6 +10,7 @@ import com.iflytop.gd.common.enums.AcidPumpDeviceCode; import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.common.enums.cmd.CmdAxis; import com.iflytop.gd.common.enums.data.DevicePositionCode; +import com.iflytop.gd.hardware.exception.HardwareException; import com.iflytop.gd.hardware.service.GDDeviceStatusService; import com.iflytop.gd.hardware.type.IO.InputIOMId; import com.iflytop.gd.hardware.type.LiquidDistributionArmDriver; @@ -549,6 +550,21 @@ public class DeviceCommandUtilService { commandWait(deviceCommandFuture); } + /** + * 获取指定加热模块是否有托盘 + */ + public Boolean heatModuleTray(HeatModuleCode heatModuleCode) throws HardwareException { + return switch (heatModuleCode) { + case heat_module_01 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE1_EXSIT); + case heat_module_02 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE2_EXSIT); + case heat_module_03 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE3_EXSIT); + case heat_module_04 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE4_EXSIT); + case heat_module_05 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE5_EXSIT); + case heat_module_06 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE6_EXSIT); + }; + + } + public void commandWait(CommandFuture... futures) throws Exception { CompletableFuture[] responseFutures = Arrays.stream(futures) diff --git a/src/main/java/com/iflytop/gd/common/cmd/CommandHandler.java b/src/main/java/com/iflytop/gd/common/cmd/CommandHandler.java index 3bb0af2..811dcaf 100644 --- a/src/main/java/com/iflytop/gd/common/cmd/CommandHandler.java +++ b/src/main/java/com/iflytop/gd/common/cmd/CommandHandler.java @@ -5,5 +5,5 @@ import com.iflytop.gd.app.model.dto.CmdDTO; import java.util.concurrent.CompletableFuture; public interface CommandHandler { - CompletableFuture handle(CmdDTO cmdDTO); + CompletableFuture handle(CmdDTO cmdDTO) throws Exception; } \ No newline at end of file diff --git a/src/main/java/com/iflytop/gd/common/result/ResultCode.java b/src/main/java/com/iflytop/gd/common/result/ResultCode.java index 62146ff..23c6062 100644 --- a/src/main/java/com/iflytop/gd/common/result/ResultCode.java +++ b/src/main/java/com/iflytop/gd/common/result/ResultCode.java @@ -47,7 +47,12 @@ public enum ResultCode implements IResultCode, Serializable { COMMAND_EXEC_TIMEOUT("5003", "命令执行超时"), HARDWARE_ERROR("5004", "硬件错误"), //============================ 6xxx：指令相关 ============================ - COMMAND_NOT_FOUND("6000", "指令未找到"); + COMMAND_NOT_FOUND("6000", "指令未找到"), + COMMAND_ALREADY_EXECUTING("6001", "指令正在执行，无法重复执行"), + SENSOR_STATUS_FAILED("6010", "获取传感器状态失败"), + TARGET_HEAT_MODULE_OCCUPIED("6021", "目标加热模块被占用"), + TARGET_HEAT_MODULE_NO_TRAY("6022", "目标加热模块无托盘"); + /** 状态码 */ private final String code; /** 提示信息 */