diff --git a/src/main/java/com/iflytop/sgs/app/cmd/control/CleanStartCommand.java b/src/main/java/com/iflytop/sgs/app/cmd/control/CleanStartCommand.java index 75b6f08..7da2f91 100644 --- a/src/main/java/com/iflytop/sgs/app/cmd/control/CleanStartCommand.java +++ b/src/main/java/com/iflytop/sgs/app/cmd/control/CleanStartCommand.java @@ -1,12 +1,22 @@ package com.iflytop.sgs.app.cmd.control; +import cn.hutool.core.lang.Assert; import cn.hutool.json.JSONArray; import com.iflytop.sgs.app.core.BaseCommandHandler; +import com.iflytop.sgs.app.model.bo.Point3D; +import com.iflytop.sgs.app.model.bo.status.device.HeatModuleState; +import com.iflytop.sgs.app.model.bo.status.device.SolutionModuleState; import com.iflytop.sgs.app.model.dto.CmdDTO; +import com.iflytop.sgs.app.service.api.DevicePositionService; import com.iflytop.sgs.app.service.device.DeviceStateService; 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; +import com.iflytop.sgs.common.enums.data.DevicePositionCode; +import com.iflytop.sgs.common.exception.AppException; +import com.iflytop.sgs.common.result.ResultCode; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -21,13 +31,60 @@ import java.util.concurrent.CompletableFuture; @RequiredArgsConstructor @CommandMapping("heat_start")//业务指令注解 public class CleanStartCommand extends BaseCommandHandler { - private final HeatModuleService heatModuleService; + private final SolutionModuleService solutionModuleService; + private final DevicePositionService devicePositionService; + private final TransferModuleService transferModuleService; private final DeviceStateService deviceStateService; + private final HeatModuleService heatModuleService; @Override public CompletableFuture handle(CmdDTO cmdDTO) { - JSONArray heatIdJsonArray = cmdDTO.getJSONArrayParam("heatId"); + JSONArray jsonArray=cmdDTO.getJSONArrayParam("index"); + String heatId = cmdDTO.getStringParam("heatId"); + HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId); + HeatModuleState heatModuleState=deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleId); + SolutionModuleState solutionModuleState=deviceStateService.getDeviceState().getSolutionModule(); + //转运模块Z轴拿取托盘时起下降的高度位置 + Point3D transferModuleZPickTrayDownPositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZPickTrayDownPositon).getPoint3D(); + //转运模块Z轴移动托盘时起托盘的高度位置 + Point3D transferModuleZTrayUpMovePositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZTrayUpMovePositon).getPoint3D(); + //转运模块X轴拿取托盘进出卡槽移动距离 + double transferModuleXPickTrayMoveDistance=devicePositionService.getPosition(DevicePositionCode.transferModuleXPickTrayMoveDistance).getDistance(); + //获取指定加热模块上方点位 + Point3D heatAreaTrayPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleId); + //获取加液区上方点位 + Point3D liquidAreaTrayPoint = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D(); + //判断加热区有无托盘 + Assert.isTrue(heatModuleState.getTrayStatus(),new AppException(ResultCode.TARGET_HEAT_MODULE_NO_TRAY,ResultCode.TARGET_HEAT_MODULE_NO_TRAY.getMsg())); + return runAsync(() -> { + //机械臂移动到加液区位置 + transferModuleService.transferMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); + //加液臂z下降 + transferModuleService.transferZMoveBy(clawTrayHeight); + //加液臂x左移 夹紧托盘 + transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip); + //加液臂z上移动 + transferModuleService.transferZMoveBy(-clawTrayHeight); + for (int i=0;i handle(CmdDTO cmdDTO) { - JSONArray heatIdJsonArray = cmdDTO.getJSONArrayParam("heatId"); + return runAsync(() -> { + }); } } 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 57b4e42..18ee698 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 @@ -56,40 +56,30 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId); HeatModuleState heatModuleState=deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleId); SolutionModuleState solutionModuleState=deviceStateService.getDeviceState().getSolutionModule(); - //判断是否有托盘 - Assert.isTrue(heatModuleState.getTrayStatus()==0,heatModuleId+"此加热位已存在托盘"); //判断泵现在处于关闭状态 Assert.isTrue(solutionModuleState.isPumping(),"蠕动泵正在运行，无法转移"); - //加液电机升起的安全高度 - double liquidMotorSafeDistance = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance(); - //获取机械臂夹取托盘的横向距离 - double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance(); - //获取机械臂夹取托盘的纵向高度 - double clawTrayHeight = devicePositionService.getPosition(DevicePositionCode.clawTrayHeight).getDistance(); + //转运模块Z轴拿取托盘时起下降的高度位置 + Point3D transferModuleZPickTrayDownPositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZPickTrayDownPositon).getPoint3D(); + //转运模块Z轴移动托盘时起托盘的高度位置 + Point3D transferModuleZTrayUpMovePositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZTrayUpMovePositon).getPoint3D(); + //转运模块X轴拿取托盘进出卡槽移动距离 + double transferModuleXPickTrayMoveDistance=devicePositionService.getPosition(DevicePositionCode.transferModuleXPickTrayMoveDistance).getDistance(); //获取指定加热模块上方点位 - Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleId); - //获取加液模块的上方点位 - Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D(); + Point3D heatAreaTrayPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleId); return runAsync(() -> { try { - //升高加液电机高度 - solutionModuleService.motorLiquidMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidMotorSafeDistance); - //移动到加液区点位 - transferModuleService.transferMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); - //下降z轴 - transferModuleService.transferZMove(clawTrayHeight); - //向左夹紧托盘 - transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip); + //升高加液电机高度 回0点 + solutionModuleService.motorLiquidMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0); //移动机械臂到加热区上方 此时机械臂夹着托盘 - transferModuleService.transferMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D); + transferModuleService.transferMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayPoint3D); //下降z轴 - transferModuleService.transferZMove(clawTrayHeight); + transferModuleService.transferZMove(transferModuleZPickTrayDownPositon.getZ()); //移动x轴向右脱离托盘 - transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawTrayGrip); + transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -transferModuleXPickTrayMoveDistance); //z轴返回0点 solutionModuleService.motorLiquidMove(cmdDTO.getCommandId(), cmdDTO.getCommand(),0); //设置加热区有托盘 - deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleId).setTrayStatus(1); + deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleId).setTrayStatus(true); } finally { deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false); } 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 02bddd9..18b9f0d 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 @@ -1,7 +1,10 @@ package com.iflytop.sgs.app.cmd.control; +import cn.hutool.core.lang.Assert; import com.iflytop.sgs.app.core.BaseCommandHandler; import com.iflytop.sgs.app.model.bo.Point3D; +import com.iflytop.sgs.app.model.bo.status.device.HeatModuleState; +import com.iflytop.sgs.app.model.bo.status.device.SolutionModuleState; import com.iflytop.sgs.app.model.dto.CmdDTO; import com.iflytop.sgs.app.service.api.DevicePositionService; import com.iflytop.sgs.app.service.device.DeviceStateService; @@ -44,48 +47,36 @@ public class MoveToLiquidAreaCommand extends BaseCommandHandler { } String heatId = cmdDTO.getStringParam("heatId"); HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId); - //加液电机升起的安全高度 - double liquidMotorSafeDistance = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance(); - //获取机械臂夹取托盘的横向距离 - double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance(); - //获取机械臂夹取托盘的纵向高度 - double clawTrayHeight = devicePositionService.getPosition(DevicePositionCode.clawTrayHeight).getDistance();//获取z轴夹取托盘的下降高度 - //获取加液模块上方点位 - Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D(); + HeatModuleState heatModuleState=deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleId); + SolutionModuleState solutionModuleState=deviceStateService.getDeviceState().getSolutionModule(); + //转运模块Z轴拿取托盘时起下降的高度位置 + Point3D transferModuleZPickTrayDownPositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZPickTrayDownPositon).getPoint3D(); + //转运模块Z轴移动托盘时起托盘的高度位置 + Point3D transferModuleZTrayUpMovePositon = devicePositionService.getPosition(DevicePositionCode.transferModuleZTrayUpMovePositon).getPoint3D(); + //转运模块X轴拿取托盘进出卡槽移动距离 + double transferModuleXPickTrayMoveDistance=devicePositionService.getPosition(DevicePositionCode.transferModuleXPickTrayMoveDistance).getDistance(); //获取指定加热模块上方点位 Point3D heatAreaTrayPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleId); - - //校验目标加热位是否有托盘 - try { - deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(true); - } catch (Exception e) { - deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false); - throw e; - } + //获取加液区上方点位 + Point3D liquidAreaTrayPoint = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D(); return runAsync(() -> { try { - //升高加液电机高度 - solutionModuleService.motorLiquidMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidMotorSafeDistance); - //机械臂移动到加热位上方 + //升高加液电机高度 回0点 + solutionModuleService.motorLiquidMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0); + //移动机械臂到加热区上方 此时机械臂夹着托盘 transferModuleService.transferMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayPoint3D); //下降z轴 - transferModuleService.transferZMove(clawTrayHeight); - //移动x轴向左夹紧托盘 - transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip); - //z轴上升到0点 todo 安全距离即可 需要测试 - transferModuleService.transferZMove(100); - //机械臂移动到加液区 - transferModuleService.transferMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); - //下降z轴 - transferModuleService.transferZMove(clawTrayHeight); - //移动x轴向右放开托盘 - transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawTrayGrip); - //z轴上升到0点 todo 安全距离即可 需要测试 - transferModuleService.transferZMove(100); - //todo x轴回原点 - transferModuleService.transferXMoveOrigin(cmdDTO.getCommandId(), cmdDTO.getCommand()); + transferModuleService.transferZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(),transferModuleZPickTrayDownPositon.getZ()); + //移动x轴向左托起托盘 + transferModuleService.transferXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), transferModuleXPickTrayMoveDistance); + //z轴返回0点 + solutionModuleService.motorLiquidMove(cmdDTO.getCommandId(), cmdDTO.getCommand(),0); + //移动到建邺区 + transferModuleService.transferMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint); + //设置加液区有托盘 todo + //deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(1); } finally { - deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false); + deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false); } }); }