From b1946d4d840b3b61c60aeeea5e0d7a12233881be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=99=BD=E5=87=A4=E5=90=89?= Date: Sun, 15 Jun 2025 10:53:45 +0800 Subject: [PATCH] =?UTF-8?q?feat:=E5=B7=A5=E8=89=BA=E9=81=BF=E8=AE=A9?= =?UTF-8?q?=E7=A7=BB=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../gd/app/service/crafts/CraftsStepService.java | 178 +++------------------ .../app/service/device/ActionCommandService.java | 15 ++ 2 files changed, 37 insertions(+), 156 deletions(-) diff --git a/src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java b/src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java index 718dd16..35b8a69 100644 --- a/src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java +++ b/src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java @@ -16,6 +16,7 @@ import com.iflytop.gd.app.model.entity.Photos; import com.iflytop.gd.app.service.api.ContainerService; import com.iflytop.gd.app.service.api.DevicePositionService; import com.iflytop.gd.app.service.api.PhotosService; +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; @@ -69,6 +70,7 @@ public class CraftsStepService { private final DeviceCommandTempUtilService deviceCommandTempUtilService; private final CameraBaslerDriver cameraBaslerDriver; private final PhotosService photosService; + private final ActionCommandService actionCommandService; @Value("${photo.url}") private String url; @@ -396,170 +398,34 @@ public class CraftsStepService { * 将托盘移至加热 */ private void moveToHeatModule(HeatModuleCode heatModuleCode) throws Exception { - 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();//获取拍子存放区上方点位; - - TrayState trayState = deviceStateService.getDeviceState().getTrayInSolutionModule(); - log.info("工艺{},提升拍子存放区至拍子夹取的高度", heatModuleCode); - capModuleService.capUpBalanceNoWait();//提升拍子存放区至拍子夹取的高度 - log.info("工艺{},将机械臂移动至加液模块上方", heatModuleCode); - gantryModuleService.gantryXYMove(liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方 - log.info("工艺{},将夹爪打开，准备夹取托盘", heatModuleCode); - gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，准备夹取托盘 - log.info("工艺{},下降z轴,使夹爪落入托盘孔位", heatModuleCode); - gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位 - log.info("工艺{},将夹爪收紧，夹住托盘", heatModuleCode); - gantryModuleService.clawMove(clawTrayGrip);//将夹爪收紧，夹住托盘 - log.info("工艺{},抬升z轴", heatModuleCode); -// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块 - gantryModuleService.gantryZMove(0);//抬升z轴 - deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(0);//加液模块是否有托盘 - deviceStateService.getDeviceState().getTrayStateByHeatModuleCode(heatModuleCode).setInSolutionModule(false); - log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode); -// gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方 - log.info("工艺{},将携带托盘的机械臂移动至加热模块上方", heatModuleCode); - gantryModuleService.gantryXYMove(heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方 - log.info("工艺{},临时避让完毕，升起", heatModuleCode); - deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块 - Thread.sleep(3200); - log.info("工艺{},下降z轴,使托盘落入加热模块", heatModuleCode); - gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使托盘落入加热模块 - - log.info("工艺{},将夹爪打开，释放托盘", heatModuleCode); - gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，释放托盘 - deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(1);//加热模块是否存在托盘 - deviceStateService.getDeviceState().getTrayStateByHeatModuleCode(heatModuleCode).setInHeatModule(true); - log.info("工艺{},抬升z轴", heatModuleCode); - gantryModuleService.gantryZMove(0);//抬升z轴 - - log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode); - gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 -// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块 - log.info("工艺{},将夹爪打开，准备夹取拍子", heatModuleCode); - gantryModuleService.clawMove(clawCapPick);//将夹爪打开，准备夹取拍子 - log.info("工艺{},下降z轴,使夹爪落入拍子升降模块拍子孔位", heatModuleCode); - gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹爪落入拍子升降模块拍子孔位 - log.info("工艺{},将夹爪收紧，夹住拍子", heatModuleCode); - gantryModuleService.clawMove(clawCapGrip);//将夹爪收紧，夹住拍子 - log.info("工艺{},抬升z轴", heatModuleCode); - gantryModuleService.gantryZMove(0);//抬升z轴 - log.info("工艺{},将携带拍子的机械臂移动至加热模块拍子上方", heatModuleCode); - gantryModuleService.gantryXYMove(heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方 - log.info("工艺{},提升拍子存放区", heatModuleCode); - capModuleService.capUpBalanceNoWait(); //提升拍子存放区 -// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode); -// Thread.sleep(4000);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块 - log.info("工艺{},下降z轴,使夹拍子落入加热模块", heatModuleCode); - gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹拍子落入加热模块 - log.info("工艺{},将夹爪打开，释放拍子", heatModuleCode); - gantryModuleService.clawMove(clawCapPick);//将夹爪打开，释放拍子 - deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(true);//加热模块是否存在拍子 - log.info("工艺{},抬升z轴", heatModuleCode); - gantryModuleService.gantryZMove(0);//抬升z轴 - log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode); - gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 - log.info("工艺{},释放加液区", heatModuleCode); - solutionModuleService.releaseSolutionModule();//释放加液区 + try { + capModuleService.capUpBalanceNoWait();//提升拍子存放区至拍子夹取的高度 + actionCommandService.moveTrayToHeatModule(heatModuleCode);//将托盘移动至指定加热模块 + actionCommandService.heatModuleInstallCap(heatModuleCode);//指定加热模块安装拍子 + gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + } finally { + deviceStateService.getDeviceState().getGantryArm().setIdle(true);//机械臂空闲 + deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false);//该指令结束 + solutionModuleService.releaseSolutionModule();//释放加液区等待 + } } /** * 将托盘移至加液 */ private void moveToSolutionModule(HeatModuleCode heatModuleCode) throws Exception { - 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();//获取拍子存放区上方点位; - - TrayState trayState = deviceStateService.getDeviceState().getTrayStateByHeatModuleCode(heatModuleCode); - log.info("工艺{},申请使用加液区并等待", heatModuleCode); - solutionModuleService.requestSolutionModule();//申请使用加液区并等待 - log.info("工艺{},提升拍子存放区至拍子夹取的高度", heatModuleCode); - capModuleService.capUpBalanceNoWait(); //提升拍子存放区至拍子夹取的高度 - log.info("工艺{},将机械臂移动至加热模块拍子上方", heatModuleCode); - gantryModuleService.gantryXYMove(heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方 - deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起 - log.info("工艺{},抬升指定加热位托盘", heatModuleCode); - heatModuleService.heaterMotorMove(heatModuleCode, trayLift);//抬升指定加热位托盘 - log.info("工艺{},将夹爪打开，准备夹取拍子", heatModuleCode); - gantryModuleService.clawMove(clawCapPick);//将夹爪打开，准备夹取拍子 - log.info("工艺{},下降z轴,使夹爪落入加热模块拍子孔位", heatModuleCode); - gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位 - log.info("工艺{},将夹爪收紧，夹住拍子", heatModuleCode); - gantryModuleService.clawMove(clawCapGrip);//将夹爪收紧，夹住拍子 - deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子 - log.info("工艺{},拍子存放模块下降1个拍子位置", heatModuleCode); - capModuleService.capMotorMoveByNumNoWait(-1);//拍子存放模块下降1个拍子位置 -// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块 - log.info("工艺{},抬升z轴", heatModuleCode); - gantryModuleService.gantryZMove(0); - log.info("工艺{},将携带拍子的机械臂移动至存放区上方", heatModuleCode); - gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方 -// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(null);//TODO 临时避 恢复抬起状态 - log.info("工艺{},下降z轴,使夹拍子落入存放区", heatModuleCode); - gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区 - log.info("工艺{},将夹爪打开，释放夹取的拍子", heatModuleCode); - gantryModuleService.clawMove(clawCapPick);//将夹爪打开，释放夹取的拍子 - log.info("工艺{},抬升z轴", heatModuleCode); - gantryModuleService.gantryZMove(0);//抬升z轴 - - log.info("工艺{},将机械臂移动至加热模块上方", heatModuleCode); - gantryModuleService.gantryXYMove(heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方 - log.info("工艺{},将夹爪打开，准备夹取托盘", heatModuleCode); - gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，准备夹取托盘 - log.info("工艺{},下降z轴,使夹爪落入托盘孔位", heatModuleCode); - gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位 - log.info("工艺{},将夹爪收紧，夹住托盘", heatModuleCode); - gantryModuleService.clawMove(clawTrayGrip);//将夹爪收紧，夹住托盘 - deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(0);//加热模块是否存在托盘 - trayState.setInHeatModule(false); - log.info("工艺{},临时避让下降", heatModuleCode); - deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(heatModuleCode);//TODO 临时避让下降 - log.info("工艺{},抬升z轴", heatModuleCode); - gantryModuleService.gantryZMoveZero();//抬升z轴 - Thread.sleep(2500);//抬升z轴 - log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode); -// gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方 - log.info("工艺{},将携带托盘的机械臂移动至加液模块上方", heatModuleCode); - gantryModuleService.gantryXYMove(liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方 - log.info("工艺{},临时避 恢复抬起状态", heatModuleCode); - deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(null, heatModuleCode);//TODO 临时避 恢复抬起状态 - log.info("工艺{},下降z轴,使托盘落入加液模块", heatModuleCode); - gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块 - deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(1);//加液模块是否存在托盘 - trayState.setInSolutionModule(true); - log.info("工艺{},将夹爪打开，释放托盘", heatModuleCode); - gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，释放托盘 - log.info("工艺{},抬升z轴", heatModuleCode); - gantryModuleService.gantryZMoveZero();//抬升z轴 - log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode); - gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + try { + solutionModuleService.requestSolutionModule();//申请使用加液区并等待 + capModuleService.capUpBalanceNoWait(); //提升拍子存放区至拍子夹取的高度 + actionCommandService.heatModuleRemoveCap(heatModuleCode);//指定加热模块移除拍子 + actionCommandService.heatModuleRemoveTray(heatModuleCode);//指定加热模块移除托盘 + gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + } finally { + deviceStateService.getDeviceState().getGantryArm().setIdle(true);//机械臂空闲 + deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false);//该指令结束 + } } } 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 index b29839a..e3404f9 100644 --- a/src/main/java/com/iflytop/gd/app/service/device/ActionCommandService.java +++ b/src/main/java/com/iflytop/gd/app/service/device/ActionCommandService.java @@ -28,6 +28,9 @@ public class ActionCommandService { private final GantryModuleService gantryModuleService; private final DeviceCommandTempUtilService deviceCommandTempUtilService; + public void heatModuleRemoveTray(HeatModuleCode heatModuleCode) throws Exception { + heatModuleRemoveTray(null, null, heatModuleCode); + } /** * 指定加热模块移除托盘 @@ -68,6 +71,10 @@ public class ActionCommandService { gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴 } + public void heatModuleRemoveCap(HeatModuleCode heatModuleCode) throws Exception { + heatModuleRemoveCap(null, null, heatModuleCode); + } + /** * 指定加热模块移除拍子 */ @@ -100,6 +107,10 @@ public class ActionCommandService { gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴 } + public void heatModuleInstallCap(HeatModuleCode heatModuleCode) throws Exception { + heatModuleInstallCap(null, null, heatModuleCode); + } + /** * 指定加热模块安装拍子 */ @@ -131,6 +142,10 @@ public class ActionCommandService { gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴 } + public void moveTrayToHeatModule(HeatModuleCode heatModuleCode) throws Exception { + moveTrayToHeatModule(null, null, heatModuleCode); + } + /** * 从加液模块移动托盘至指定加热模块 */