diff --git a/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToFeedAreaCommand.java b/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToFeedAreaCommand.java index eab00d0..e8b82da 100644 --- a/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToFeedAreaCommand.java +++ b/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToFeedAreaCommand.java @@ -52,7 +52,6 @@ public class MoveToFeedAreaCommand extends BaseCommandHandler { Point3D targetHeatModuleTrayClawPoint3D = heatModuleService.getHeatModuleTrayClawPoint3D(targetHeatModuleCode);//获取目标加热模块托盘夹取点 Double transferModuleXPickTrayMoveDistance = devicePositionService.getPosition(DevicePositionCode.transferModuleXPickTrayMoveDistance).getDistance();//获取转运模块X轴拿取托盘进出卡槽移动距离 - Double transferModuleZPickTrayDownPositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZPickTrayDownPositon).getPositon();//获取转运模块Z轴拿取托盘时下降的高度位置 Point3D feedAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.feedAreaTrayPoint).getPoint3D();//获取上料区托盘夹爪位置点 return runAsync(() -> { @@ -61,21 +60,21 @@ public class MoveToFeedAreaCommand extends BaseCommandHandler { //获取加液位托盘 deviceStateService.getDeviceState().getTrayByHeatModuleCode(targetHeatModuleCode).setInSolutionPositon(false);//设定托盘不在加液位中 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getX());//将X轴移动至上料区位置点 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入上料区 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getZ());//Z轴下降至夹取点，使托盘落入上料区 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleXPickTrayMoveDistance);//X轴移出卡槽 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 } else { //获取目标加热模块托盘 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getX() + transferModuleXPickTrayMoveDistance);//将X轴移动至目标加热模块托盘夹取点 + 进出卡槽移动距离 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入石墨加热盘 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getZ());//Z轴下降至夹取点，使托盘落入石墨加热盘 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -transferModuleXPickTrayMoveDistance);//X轴进入卡槽 deviceStateService.getDeviceState().getHeatModuleByCode(targetHeatModuleCode).setTrayStatus(false);//设定目标加热模块托盘状态 deviceStateService.getDeviceState().getTrayByHeatModuleCode(targetHeatModuleCode).setInHeatModule(false);//设定托盘不在加热模块中 deviceStateService.getDeviceState().getTransferModule().setTrayStatus(true);//设定托盘在机械臂上 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getX());//将X轴移动至上料区位置点 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入上料区 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getZ());//Z轴下降至夹取点，使托盘落入上料区 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleXPickTrayMoveDistance);//X轴移出卡槽 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 } diff --git a/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToHeatAreaCommand.java b/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToHeatAreaCommand.java index a99a86b..2b15f3f 100644 --- a/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToHeatAreaCommand.java +++ b/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToHeatAreaCommand.java @@ -41,7 +41,6 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { HeatModuleCode targetHeatModuleCode = HeatModuleCode.valueOf(targetHeatModuleCodeStr);//目标加热模块 Point3D targetHeatModuleTrayClawPoint3D = heatModuleService.getHeatModuleTrayClawPoint3D(targetHeatModuleCode);//获取目标加热模块托盘夹取点 Double transferModuleXPickTrayMoveDistance = devicePositionService.getPosition(DevicePositionCode.transferModuleXPickTrayMoveDistance).getDistance();//获取转运模块X轴拿取托盘进出卡槽移动距离 - Double transferModuleZPickTrayDownPositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZPickTrayDownPositon).getPositon();//获取转运模块Z轴拿取托盘时下降的高度位置 Point3D feedAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.feedAreaTrayPoint).getPoint3D();//获取上料区托盘夹爪位置点 return runAsync(() -> { @@ -50,13 +49,13 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { if (!liquidTrayExist) { //判断机械臂上是否有托盘 //机械臂上无托盘，取上料区托盘 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getX() + transferModuleXPickTrayMoveDistance);//将X轴移动至上料区托盘夹取点 + 进出卡槽移动距离 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入石墨加热盘 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getZ());//Z轴下降至夹取点，使托盘落入石墨加热盘 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -transferModuleXPickTrayMoveDistance);//X轴进入卡槽 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 } transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getX());//将X轴移动至目标加热模块托盘夹取点 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入石墨加热盘 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getZ());//Z轴下降至夹取点，使托盘落入石墨加热盘 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleXPickTrayMoveDistance);//X轴移出卡槽 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 deviceStateService.getDeviceState().getHeatModuleByCode(targetHeatModuleCode).setTrayStatus(true);//设置目标加热区有托盘 diff --git a/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToLiquidAreaCommand.java b/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToLiquidAreaCommand.java index 8602dc4..37f4231 100644 --- a/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToLiquidAreaCommand.java +++ b/src/main/java/com/iflytop/sgs/app/cmd/control/MoveToLiquidAreaCommand.java @@ -49,7 +49,6 @@ public class MoveToLiquidAreaCommand extends BaseCommandHandler { Point3D targetHeatModuleTrayClawPoint3D = heatModuleService.getHeatModuleTrayClawPoint3D(targetHeatModuleCode);//获取目标加热模块托盘夹取点 Double transferModuleXPickTrayMoveDistance = devicePositionService.getPosition(DevicePositionCode.transferModuleXPickTrayMoveDistance).getDistance();//获取转运模块X轴拿取托盘进出卡槽移动距离 - Double transferModuleZPickTrayDownPositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZPickTrayDownPositon).getPositon();//获取转运模块Z轴拿取托盘时下降的高度位置 Point3D feedAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.feedAreaTrayPoint).getPoint3D();//获取上料区托盘夹爪位置点 Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液时托盘位置点 @@ -58,7 +57,7 @@ public class MoveToLiquidAreaCommand extends BaseCommandHandler { if (targetHeatModuleCode == null) { //获取上料区托盘 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getX() + transferModuleXPickTrayMoveDistance);//将X轴移动至上料区托盘夹取点 + 进出卡槽移动距离 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入石墨加热盘 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getZ());//Z轴下降至夹取点，使托盘落入石墨加热盘 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -transferModuleXPickTrayMoveDistance);//X轴进入卡槽 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 deviceStateService.getDeviceState().getSolutionModule().setFeedAreaTrayStatus(false);//设定上料区托盘状态 @@ -66,7 +65,7 @@ public class MoveToLiquidAreaCommand extends BaseCommandHandler { } else { //获取目标加热模块托盘 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getX() + transferModuleXPickTrayMoveDistance);//将X轴移动至目标加热模块托盘夹取点 + 进出卡槽移动距离 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入石墨加热盘 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getZ());//Z轴下降至夹取点，使托盘落入石墨加热盘 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -transferModuleXPickTrayMoveDistance);//X轴进入卡槽 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 deviceStateService.getDeviceState().getHeatModuleByCode(targetHeatModuleCode).setTrayStatus(false);//设定目标加热模块托盘状态 diff --git a/src/main/java/com/iflytop/sgs/app/cmd/selftest/MoveTestCommand.java b/src/main/java/com/iflytop/sgs/app/cmd/selftest/MoveTestCommand.java index cb3f8e6..6266d54 100644 --- a/src/main/java/com/iflytop/sgs/app/cmd/selftest/MoveTestCommand.java +++ b/src/main/java/com/iflytop/sgs/app/cmd/selftest/MoveTestCommand.java @@ -30,7 +30,6 @@ public class MoveTestCommand extends BaseCommandHandler { private final SolutionModuleService solutionModuleService; private final TransferModuleService transferModuleService; private final HeatModuleService heatModuleService; - private final DeviceStateService deviceStateService; private final DevicePositionService devicePositionService; private final WebSocketSender webSocketService; @@ -38,7 +37,6 @@ public class MoveTestCommand extends BaseCommandHandler { @Override public CompletableFuture handle(CmdDTO cmdDTO) { HeatModuleCode annealHeatModuleCode = HeatModuleCode.heat_module_04; - Double transferModuleZPickTrayDownPositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZPickTrayDownPositon).getPositon();//获取转运模块Z轴拿取托盘时下降的高度位置 Point3D annealHeatModuleTrayClawPoint3D = heatModuleService.getHeatModuleTrayClawPoint3D(annealHeatModuleCode);//获取退火加热模块托盘夹取点 Double solutionModuleMotorDownInTubeExtPositon = devicePositionService.getPosition(DevicePositionCode.solutionModuleMotorDownInTubeExtPositon).getPositon();//加液模块电机下降进入试管抽取位置 Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液时托盘位置点 @@ -46,14 +44,16 @@ public class MoveTestCommand extends BaseCommandHandler { return runAsync(() -> { webSocketService.pushSelfMoveTest(SelfMoveTestGenerator.generateJson(cmdDTO.getCommandId(), cmdDTO.getCommand(), "1、各项传感器正常", 25)); + solutionModuleService.solutionMotorMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//加液机械臂抬升至最高 + transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), annealHeatModuleTrayClawPoint3D.getX());//将X轴移动至托盘夹取点 solutionModuleService.solutionMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), solutionModuleMotorDownInTubeExtPositon);//加液模块下降进入试管抽取位置 - solutionModuleService.solutionMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//加液机械臂上升 + solutionModuleService.solutionMotorMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//加液机械臂抬升至最高 webSocketService.pushSelfMoveTest(SelfMoveTestGenerator.generateJson(cmdDTO.getCommandId(), cmdDTO.getCommand(), "2、加液模块升降电机检测完毕", 50)); transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D.getX());//将X轴移动至加液时托盘位置点 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getX());//将X轴移动至上料区位置点 webSocketService.pushSelfMoveTest(SelfMoveTestGenerator.generateJson(cmdDTO.getCommandId(), cmdDTO.getCommand(), "3、x轴电机检测完毕", 75)); - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入上料区 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getZ());//Z轴下降至夹取点，使托盘落入上料区 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 webSocketService.pushSelfMoveTest(SelfMoveTestGenerator.generateJson(cmdDTO.getCommandId(), cmdDTO.getCommand(), "4、z轴电机检测完毕", 100)); }); diff --git a/src/main/java/com/iflytop/sgs/app/cmd/selftest/TrayOutCommand.java b/src/main/java/com/iflytop/sgs/app/cmd/selftest/TrayOutCommand.java index e439487..ba6d741 100644 --- a/src/main/java/com/iflytop/sgs/app/cmd/selftest/TrayOutCommand.java +++ b/src/main/java/com/iflytop/sgs/app/cmd/selftest/TrayOutCommand.java @@ -7,6 +7,7 @@ import com.iflytop.sgs.app.service.api.DevicePositionService; import com.iflytop.sgs.app.service.device.DeviceStateService; import com.iflytop.sgs.app.service.device.module.DoorModuleService; import com.iflytop.sgs.app.service.device.module.HeatModuleService; +import com.iflytop.sgs.app.service.device.module.SolutionModuleService; import com.iflytop.sgs.app.service.device.module.TransferModuleService; import com.iflytop.sgs.common.annotation.CommandMapping; import com.iflytop.sgs.common.enums.HeatModuleCode; @@ -27,6 +28,7 @@ import java.util.concurrent.CompletableFuture; public class TrayOutCommand extends BaseCommandHandler { private final DoorModuleService doorModuleService; private final TransferModuleService transferModuleService; + private final SolutionModuleService solutionModuleService; private final HeatModuleService heatModuleService; private final DevicePositionService devicePositionService; private final DeviceStateService deviceStateService; @@ -39,23 +41,22 @@ public class TrayOutCommand extends BaseCommandHandler { Double doorOpenPosition = devicePositionService.getPosition(DevicePositionCode.doorOpenPosition).getDistance();//获取开门位置 Point3D targetHeatModuleTrayClawPoint3D = heatModuleService.getHeatModuleTrayClawPoint3D(targetHeatModuleCode);//获取目标加热模块托盘夹取点 Double transferModuleXPickTrayMoveDistance = devicePositionService.getPosition(DevicePositionCode.transferModuleXPickTrayMoveDistance).getDistance();//获取转运模块X轴拿取托盘进出卡槽移动距离 - Double transferModuleZPickTrayDownPositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZPickTrayDownPositon).getPositon();//获取转运模块Z轴拿取托盘时下降的高度位置 Point3D feedAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.feedAreaTrayPoint).getPoint3D();//获取上料区托盘夹爪位置点 return runAsync(() -> { if (!deviceStateService.getDeviceState().getDoorModule().isOpen()) { doorModuleService.doorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), doorOpenPosition); deviceStateService.getDeviceState().getDoorModule().setOpen(true); } - + solutionModuleService.solutionMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0); transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getX() + transferModuleXPickTrayMoveDistance);//将X轴移动至目标加热模块托盘夹取点 + 进出卡槽移动距离 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入石墨加热盘 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), targetHeatModuleTrayClawPoint3D.getZ());//Z轴下降至夹取点，使托盘落入石墨加热盘 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -transferModuleXPickTrayMoveDistance);//X轴进入卡槽 deviceStateService.getDeviceState().getHeatModuleByCode(targetHeatModuleCode).setTrayStatus(false);//设定目标加热模块托盘状态 deviceStateService.getDeviceState().getTrayByHeatModuleCode(targetHeatModuleCode).setInHeatModule(false);//设定托盘不在加热模块中 deviceStateService.getDeviceState().getTransferModule().setTrayStatus(true);//设定托盘在机械臂上 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 transferModuleService.transferXMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getX());//将X轴移动至上料区位置点 - transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleZPickTrayDownPositon);//Z轴下降至夹取点，使托盘落入上料区 + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), feedAreaTrayPoint3D.getZ());//Z轴下降至夹取点，使托盘落入上料区 transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleXPickTrayMoveDistance);//X轴移出卡槽 transferModuleService.transferZMoveZero(cmdDTO.getCommandId(), cmdDTO.getCommand());//Z轴抬升至最高 deviceStateService.getDeviceState().getSolutionModule().setFeedAreaTrayStatus(true);//设定上料区托盘状态 diff --git a/src/main/java/com/iflytop/sgs/app/service/device/module/SolutionModuleService.java b/src/main/java/com/iflytop/sgs/app/service/device/module/SolutionModuleService.java index 9c66974..92ff492 100644 --- a/src/main/java/com/iflytop/sgs/app/service/device/module/SolutionModuleService.java +++ b/src/main/java/com/iflytop/sgs/app/service/device/module/SolutionModuleService.java @@ -90,6 +90,22 @@ public class SolutionModuleService { /** * 加液机械臂移动到指定点 */ + public void solutionMotorMoveZero() throws Exception { + solutionMotorMoveZero(null, null); + } + + /** + * 加液机械臂移动到指定点 + */ + public void solutionMotorMoveZero(String cmdId, String cmdCode) throws Exception { + DeviceCommandBundle deviceCommand = DeviceCommandGenerator.solutionMotorMove(0.0); + CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand); + CommandUtil.wait(deviceCommandFuture); + } + + /** + * 加液机械臂移动到指定点 + */ public void solutionMotorMove(double position) throws Exception { solutionMotorMove(null, null, position); } diff --git a/src/main/java/com/iflytop/sgs/common/enums/data/DevicePositionCode.java b/src/main/java/com/iflytop/sgs/common/enums/data/DevicePositionCode.java index 2e76ad1..b2f21d1 100644 --- a/src/main/java/com/iflytop/sgs/common/enums/data/DevicePositionCode.java +++ b/src/main/java/com/iflytop/sgs/common/enums/data/DevicePositionCode.java @@ -50,10 +50,6 @@ public enum DevicePositionCode { */ solutionModuleMotorDownInTubeExtPositon(DevicePositionType.POSITION, "加液模块电机下降进入试管抽取位置"), /** - * 转运模块Z轴拿取托盘时下降的高度位置 - */ - transferModuleZPickTrayDownPositon(DevicePositionType.POSITION, "转运模块Z轴拿取托盘时起下降的高度位置"), - /** * 开门位置 */ doorOpenPosition(DevicePositionType.POSITION, "开门位置"),