From 9c48064d59d2e5314042b7d8ca07a4c39d19f151 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 13:50:44 +0800 Subject: [PATCH] =?UTF-8?q?feat:=E5=A2=9E=E5=8A=A0=E5=A4=9A=E7=82=B9?= =?UTF-8?q?=E6=8A=98=E7=BA=BF=E7=A7=BB=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../app/command/control/MoveToHeatAreaCommand.java | 11 +++--- .../command/control/MoveToSolutionAreaCommand.java | 11 +++--- .../step/DebugGantryToHeatAreaCapCommand.java | 2 +- .../step/DebugGantryToHeatAreaTrayCommand.java | 2 +- .../command/debug/step/DebugShowSmogCommand.java | 11 +----- .../gd/app/command/selftest/TrayOutCommand.java | 14 +++---- .../iflytop/gd/app/controller/TestController.java | 7 ++++ .../iflytop/gd/app/service/api/TestService.java | 29 ++++++++++++-- .../gd/app/service/crafts/CraftsStepService.java | 20 +++++----- .../service/device/DeviceStepCommandService.java | 2 +- .../service/device/module/GantryModuleService.java | 45 ++++++++++++++-------- 11 files changed, 93 insertions(+), 61 deletions(-) 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 4bf37ae..d905ee9 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 @@ -3,7 +3,6 @@ package com.iflytop.gd.app.command.control; import com.iflytop.gd.app.core.BaseCommandHandler; import com.iflytop.gd.app.model.bo.Point3D; import com.iflytop.gd.app.model.bo.status.device.HeatModuleState; -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; @@ -100,7 +99,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { trayState.setHeatModuleId(heatModuleCode); } capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand());//提升拍子存放区至拍子夹取的高度 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方 + 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);//将夹爪收紧，夹住托盘 @@ -108,7 +107,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴 deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(0);//加液模块是否有托盘 // gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方 + 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);//将夹爪打开，释放托盘 @@ -117,20 +116,20 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { trayState.setInHeatModule(true); } gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + 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.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方 + 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轴 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 } finally { deviceStateService.getDeviceState().getGantryArm().setIdle(true); deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false); 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 598d014..f583419 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 @@ -2,7 +2,6 @@ package com.iflytop.gd.app.command.control; import com.iflytop.gd.app.core.BaseCommandHandler; import com.iflytop.gd.app.model.bo.Point3D; -import com.iflytop.gd.app.model.bo.status.device.HeatModuleState; 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; @@ -105,7 +104,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler { try { solutionModuleService.requestSolutionModule();//申请使用加液区并等待 capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区至拍子夹取的高度 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方 + 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);//将夹爪打开，准备夹取拍子 @@ -115,13 +114,13 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler { 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.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方 + 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.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方 + 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);//将夹爪收紧，夹住托盘 @@ -132,7 +131,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler { 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.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方 deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(null, heatModuleCode);//TODO 临时避 恢复抬起状态 gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块 deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(1);//加液模块是否存在托盘 @@ -141,7 +140,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler { } gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，释放托盘 gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 } finally { deviceStateService.getDeviceState().getGantryArm().setIdle(true); deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false); diff --git a/src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaCapCommand.java b/src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaCapCommand.java index ef73757..5b4c428 100644 --- a/src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaCapCommand.java +++ b/src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaCapCommand.java @@ -31,7 +31,7 @@ public class DebugGantryToHeatAreaCapCommand extends BaseCommandHandler { Point3D point3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModule); point3D.setZ((double) 0); return runAsync(() -> { - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), point3D);//将携带拍子的机械臂移动至加热模块拍子上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), point3D);//将携带拍子的机械臂移动至加热模块拍子上方 }); } } diff --git a/src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaTrayCommand.java b/src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaTrayCommand.java index 08a98d2..861af40 100644 --- a/src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaTrayCommand.java +++ b/src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaTrayCommand.java @@ -31,7 +31,7 @@ public class DebugGantryToHeatAreaTrayCommand extends BaseCommandHandler { Point3D point3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModule); point3D.setZ((double) 0); return runAsync(() -> { - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), point3D); + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), point3D); }); } } diff --git a/src/main/java/com/iflytop/gd/app/command/debug/step/DebugShowSmogCommand.java b/src/main/java/com/iflytop/gd/app/command/debug/step/DebugShowSmogCommand.java index 6a6c88e..8b5a14f 100644 --- a/src/main/java/com/iflytop/gd/app/command/debug/step/DebugShowSmogCommand.java +++ b/src/main/java/com/iflytop/gd/app/command/debug/step/DebugShowSmogCommand.java @@ -4,20 +4,11 @@ import com.iflytop.gd.app.core.BaseCommandHandler; import com.iflytop.gd.app.model.bo.Point3D; import com.iflytop.gd.app.model.dto.CmdDTO; 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.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.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.data.DevicePositionCode; -import com.iflytop.gd.common.utils.CommandUtil; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -47,7 +38,7 @@ public class DebugShowSmogCommand extends BaseCommandHandler { double heatModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleCapMoveHeight).getDistance();//加热模块拍子移动高度 Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位 return runAsync(() -> { - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方 heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLift);//抬升指定加热位托盘 gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，准备夹取拍子 gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位 diff --git a/src/main/java/com/iflytop/gd/app/command/selftest/TrayOutCommand.java b/src/main/java/com/iflytop/gd/app/command/selftest/TrayOutCommand.java index 3bd38d7..185be16 100644 --- a/src/main/java/com/iflytop/gd/app/command/selftest/TrayOutCommand.java +++ b/src/main/java/com/iflytop/gd/app/command/selftest/TrayOutCommand.java @@ -6,12 +6,10 @@ 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.DeviceStateService; -import com.iflytop.gd.app.service.device.SelfTestService; import com.iflytop.gd.app.service.device.module.*; import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.common.enums.data.DevicePositionCode; -import io.swagger.v3.oas.annotations.media.Schema; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -66,7 +64,7 @@ public class TrayOutCommand extends BaseCommandHandler { // TrayState trayState = deviceStateService.getTrayInSolutionModule(); solutionModuleService.requestSolutionModule();//申请使用加液区并等待 capModuleService.capUpBalance(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区至拍子夹取的高度 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方 heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLift);//抬升指定加热位托盘 gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，准备夹取拍子 gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位 @@ -74,12 +72,12 @@ public class TrayOutCommand extends BaseCommandHandler { deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子 gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴 capModuleService.capMotorMoveByNum(-1);//拍子存放模块下降1个拍子位置 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方 gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区 gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，释放夹取的拍子 gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方 deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起 gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，准备夹取托盘 gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位 @@ -90,15 +88,15 @@ public class TrayOutCommand extends BaseCommandHandler { // deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLower);//下降加热模块托盘 // deviceStateService.setHeatModuleStateTrayUp(heatModuleId, 0);//加热模块托盘升降状态 deviceCommandTempUtilService.moveTrayHeatModuleAvoidDown(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//TODO 临时避让下降 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方 deviceCommandTempUtilService.moveTrayHeatModuleAvoidDown(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 临时避 恢复抬起状态 gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块 // trayState.setInSolutionModule(true);//托盘是否在加液模块中 deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(1);//加液模块是否存在托盘 gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，释放托盘 gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴 - gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 } finally { deviceStateService.getDeviceState().getGantryArm().setIdle(true); deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false); diff --git a/src/main/java/com/iflytop/gd/app/controller/TestController.java b/src/main/java/com/iflytop/gd/app/controller/TestController.java index dd79e3e..d4af13d 100644 --- a/src/main/java/com/iflytop/gd/app/controller/TestController.java +++ b/src/main/java/com/iflytop/gd/app/controller/TestController.java @@ -127,4 +127,11 @@ public class TestController { testService.countPackage(); return Result.success(); } + + @Operation(summary = "多点移动测试") + @PostMapping("/move-points") + public Result> moveManyPoints() throws Exception { + testService.moveManyPoints(); + return Result.success(); + } } 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 34192cf..e5d6397 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 @@ -1,8 +1,11 @@ package com.iflytop.gd.app.service.api; +import com.iflytop.gd.app.model.bo.Point3D; 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; @@ -29,6 +32,8 @@ public class TestService { private final GDDeviceStatusService gdDeviceStatusService; private final DeviceStateService deviceStateService; private final DeviceCommandService deviceCommandService; + private final HeatModuleService heatModuleService; + private final GantryModuleService gantryModuleService; public List getAllSensor() throws HardwareException { List allSensorDTOList = new ArrayList<>(); @@ -70,20 +75,38 @@ public class TestService { long endTime = System.nanoTime(); long duration = endTime - startTime; double milliseconds = duration / 1_000_000.0; - log.info("指令包装时间：{}",milliseconds); + log.info("指令包装时间：{}", milliseconds); startTime = System.nanoTime(); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(deviceCommand); endTime = System.nanoTime(); duration = endTime - startTime; milliseconds = duration / 1_000_000.0; - log.info("指令发送时间：{}",milliseconds); + log.info("指令发送时间：{}", milliseconds); CommandUtil.wait(deviceCommandFuture); endTime = System.nanoTime(); duration = endTime - startTime; milliseconds = duration / 1_000_000.0; - log.info("指令反馈时间：{}",milliseconds); + log.info("指令反馈时间：{}", milliseconds); deviceCommand = DeviceCommandGenerator.fan1Close(); deviceCommandService.sendCommand(deviceCommand); } + 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); + + gantryModuleService.moveManyPoints( + heatArea3TrayClawPoint3D, + heatArea2TrayClawPoint3D, + heatArea1TrayClawPoint3D, + heatArea4TrayClawPoint3D, + heatArea5TrayClawPoint3D, + heatArea6TrayClawPoint3D + ); + } + } 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 b2821dc..8d59085 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 @@ -351,7 +351,7 @@ public class CraftsStepService { log.info("工艺{},提升拍子存放区至拍子夹取的高度", heatModuleCode); capModuleService.capUpBalanceNoWait();//提升拍子存放区至拍子夹取的高度 log.info("工艺{},将机械臂移动至加液模块上方", heatModuleCode); - gantryModuleService.gantryMove(liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方 + gantryModuleService.gantryXYMove(liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方 log.info("工艺{},将夹爪打开，准备夹取托盘", heatModuleCode); gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，准备夹取托盘 log.info("工艺{},下降z轴,使夹爪落入托盘孔位", heatModuleCode); @@ -366,7 +366,7 @@ public class CraftsStepService { log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode); // gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方 log.info("工艺{},将携带托盘的机械臂移动至加热模块上方", heatModuleCode); - gantryModuleService.gantryMove(heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方 + gantryModuleService.gantryXYMove(heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方 log.info("工艺{},临时避让完毕，升起", heatModuleCode); deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块 Thread.sleep(3200); @@ -381,7 +381,7 @@ public class CraftsStepService { gantryModuleService.gantryZMove(0);//抬升z轴 log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode); - gantryModuleService.gantryMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 // deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块 log.info("工艺{},将夹爪打开，准备夹取拍子", heatModuleCode); gantryModuleService.clawMove(clawCapPick);//将夹爪打开，准备夹取拍子 @@ -392,7 +392,7 @@ public class CraftsStepService { log.info("工艺{},抬升z轴", heatModuleCode); gantryModuleService.gantryZMove(0);//抬升z轴 log.info("工艺{},将携带拍子的机械臂移动至加热模块拍子上方", heatModuleCode); - gantryModuleService.gantryMove(heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方 + gantryModuleService.gantryXYMove(heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方 log.info("工艺{},提升拍子存放区", heatModuleCode); capModuleService.capUpBalanceNoWait(); //提升拍子存放区 // deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode); @@ -405,7 +405,7 @@ public class CraftsStepService { log.info("工艺{},抬升z轴", heatModuleCode); gantryModuleService.gantryZMove(0);//抬升z轴 log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode); - gantryModuleService.gantryMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 log.info("工艺{},释放加液区", heatModuleCode); solutionModuleService.releaseSolutionModule();//释放加液区 } @@ -438,7 +438,7 @@ public class CraftsStepService { log.info("工艺{},提升拍子存放区至拍子夹取的高度", heatModuleCode); capModuleService.capUpBalanceNoWait(); //提升拍子存放区至拍子夹取的高度 log.info("工艺{},将机械臂移动至加热模块拍子上方", heatModuleCode); - gantryModuleService.gantryMove(heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方 + gantryModuleService.gantryXYMove(heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方 deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起 log.info("工艺{},抬升指定加热位托盘", heatModuleCode); heatModuleService.heaterMotorMove(heatModuleCode, trayLift);//抬升指定加热位托盘 @@ -455,7 +455,7 @@ public class CraftsStepService { log.info("工艺{},抬升z轴", heatModuleCode); gantryModuleService.gantryZMove(0); log.info("工艺{},将携带拍子的机械臂移动至存放区上方", heatModuleCode); - gantryModuleService.gantryMove(capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方 + gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方 // deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(null);//TODO 临时避 恢复抬起状态 log.info("工艺{},下降z轴,使夹拍子落入存放区", heatModuleCode); gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区 @@ -465,7 +465,7 @@ public class CraftsStepService { gantryModuleService.gantryZMove(0);//抬升z轴 log.info("工艺{},将机械臂移动至加热模块上方", heatModuleCode); - gantryModuleService.gantryMove(heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方 + gantryModuleService.gantryXYMove(heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方 log.info("工艺{},将夹爪打开，准备夹取托盘", heatModuleCode); gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，准备夹取托盘 log.info("工艺{},下降z轴,使夹爪落入托盘孔位", heatModuleCode); @@ -482,7 +482,7 @@ public class CraftsStepService { log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode); // gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方 log.info("工艺{},将携带托盘的机械臂移动至加液模块上方", heatModuleCode); - gantryModuleService.gantryMove(liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方 + gantryModuleService.gantryXYMove(liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方 log.info("工艺{},临时避 恢复抬起状态", heatModuleCode); deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(null, heatModuleCode);//TODO 临时避 恢复抬起状态 log.info("工艺{},下降z轴,使托盘落入加液模块", heatModuleCode); @@ -494,6 +494,6 @@ public class CraftsStepService { log.info("工艺{},抬升z轴", heatModuleCode); gantryModuleService.gantryZMove(0);//抬升z轴 log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode); - gantryModuleService.gantryMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 + gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方 } } diff --git a/src/main/java/com/iflytop/gd/app/service/device/DeviceStepCommandService.java b/src/main/java/com/iflytop/gd/app/service/device/DeviceStepCommandService.java index 70ee23d..cb7fb60 100644 --- a/src/main/java/com/iflytop/gd/app/service/device/DeviceStepCommandService.java +++ b/src/main/java/com/iflytop/gd/app/service/device/DeviceStepCommandService.java @@ -29,7 +29,7 @@ public class DeviceStepCommandService { private final GDDeviceStatusService gdDeviceStatusService; public void gantryToPoint3D(Point3D point3D) throws Exception { - gantryModuleService.gantryMove(point3D); + gantryModuleService.gantryXYMove(point3D); } /** 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 4e0f6d0..5275dfb 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 @@ -16,9 +16,6 @@ import org.springframework.stereotype.Service; import java.util.ArrayList; import java.util.List; -import java.util.concurrent.locks.Condition; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; /** * 龙门架（转运）模块 @@ -31,33 +28,51 @@ public class GantryModuleService { private final DevicePositionService devicePositionService; private final DeviceStateService deviceStateService; + + /** + * 多点依次移动 + */ + public void moveManyPoints(Point3D... point3DS) throws Exception { + moveManyPoints(null, null, point3DS); + } + + + /** + * 多点依次移动 + */ + public void moveManyPoints(String commandId, String command, Point3D... point3DS) throws Exception { + for (Point3D point3D : point3DS) { + gantryXYMove(commandId, command, point3D); + } + } + /** * 龙门架机械臂移动到0点 */ public void gantryMoveZero() throws Exception { - gantryMove(null, null, new Point3D(0.0, 0.0, 0.0)); + gantryXYMove(null, null, new Point3D(0.0, 0.0, 0.0)); } /** * 龙门架机械臂移动到指定点 */ - public void gantryMove(Point3D point) throws Exception { - gantryMove(null, null, point); + public void gantryXYMove(Point3D point) throws Exception { + gantryXYMove(null, null, point); } /** * 龙门架机械臂移动到指定点 */ - public void gantryMove(String commandId, String command, Point3D point) throws Exception { + public void gantryXYMove(String commandId, String command, Point3D point) throws Exception { DeviceCommandBundle gantryXMoveDeviceCommand = DeviceCommandGenerator.gantryXMove(point.getX()); CommandFuture gantryXMoveDeviceCommandFuture = deviceCommandService.sendCommand(commandId, command, gantryXMoveDeviceCommand); DeviceCommandBundle gantryYMoveDeviceCommand = DeviceCommandGenerator.gantryYMove(point.getY()); CommandFuture gantryYMoveDeviceCommandFuture = deviceCommandService.sendCommand(commandId, command, gantryYMoveDeviceCommand); CommandUtil.wait(gantryXMoveDeviceCommandFuture, gantryYMoveDeviceCommandFuture); - - DeviceCommandBundle gantryZMoveDeviceCommand = DeviceCommandGenerator.gantryZMove(point.getZ()); - CommandFuture gantryZMoveDeviceCommandFuture = deviceCommandService.sendCommand(commandId, command, gantryZMoveDeviceCommand); - CommandUtil.wait(gantryZMoveDeviceCommandFuture); +// +// DeviceCommandBundle gantryZMoveDeviceCommand = DeviceCommandGenerator.gantryZMove(point.getZ()); +// CommandFuture gantryZMoveDeviceCommandFuture = deviceCommandService.sendCommand(commandId, command, gantryZMoveDeviceCommand); +// CommandUtil.wait(gantryZMoveDeviceCommandFuture); } /** @@ -394,14 +409,14 @@ public class GantryModuleService { double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //托盘升降抬升距离 List futuresList = new ArrayList<>(); - for(HeatModuleCode heatModuleCode : HeatModuleCode.values()){ + for (HeatModuleCode heatModuleCode : HeatModuleCode.values()) { DeviceCommandBundle deviceCommand = getHeaterMotorMoveDeviceCommand(heatModuleCode, trayLift); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand); futuresList.add(deviceCommandFuture); } CommandUtil.wait(futuresList.toArray(new CommandFuture[0])); - for(HeatModuleCode heatModuleCode : HeatModuleCode.values()){ + for (HeatModuleCode heatModuleCode : HeatModuleCode.values()) { deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//加热模块托盘升降状态 } } @@ -411,14 +426,14 @@ public class GantryModuleService { */ public void allTrayDown(String cmdId, String cmdCode) throws Exception { List futuresList = new ArrayList<>(); - for(HeatModuleCode heatModuleCode : HeatModuleCode.values()){ + for (HeatModuleCode heatModuleCode : HeatModuleCode.values()) { DeviceCommandBundle deviceCommand = getHeaterMotorMoveDeviceCommand(heatModuleCode, 0.0); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand); futuresList.add(deviceCommandFuture); } CommandUtil.wait(futuresList.toArray(new CommandFuture[0])); - for(HeatModuleCode heatModuleCode : HeatModuleCode.values()){ + for (HeatModuleCode heatModuleCode : HeatModuleCode.values()) { deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(0);//加热模块托盘升降状态 } }