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 07f9d6c..ffa7641 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java @@ -63,7 +63,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧 double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度 deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -capSafetyHeight);//机械臂提升至移动拍子的安全高度 - deviceCommandUtilService.capMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 1); //提升拍子存放区1个位置 + 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);//将机械臂移动至拍子上方加上移动拍子的安全高度 diff --git a/src/main/java/com/iflytop/gd/app/service/CraftsStepService.java b/src/main/java/com/iflytop/gd/app/service/CraftsStepService.java index e5610fb..2bfabba 100644 --- a/src/main/java/com/iflytop/gd/app/service/CraftsStepService.java +++ b/src/main/java/com/iflytop/gd/app/service/CraftsStepService.java @@ -195,8 +195,8 @@ public class CraftsStepService { double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度 webSocketService.pushCraftsDebug(CraftsDebugGenerator.generateJson(heatId, "机械臂提升至移动拍子的安全高度", -capSafetyHeight)); deviceCommandUtilService.gantryZMoveBy(-capSafetyHeight);//机械臂提升至移动拍子的安全高度 - webSocketService.pushCraftsDebug(CraftsDebugGenerator.generateJson(heatId, "提升拍子存放区1个位置", null)); - deviceCommandUtilService.capMotorMove(1); //提升拍子存放区1个位置 + webSocketService.pushCraftsDebug(CraftsDebugGenerator.generateJson(heatId, "提升拍子存放区", null)); + deviceCommandUtilService.capUpBalance(); //提升拍子存放区 Point3D heatAreaCapClawPoint3D = deviceCommandUtilService.getHeatAreaCapClawPointPoint3D(heatModuleId);//获取托盘上方拍子点位 Point3D heatAreaCapClawSafetyHeightPoint3D = new Point3D(heatAreaCapClawPoint3D.getX(), heatAreaCapClawPoint3D.getY(), heatAreaCapClawPoint3D.getZ() - capSafetyHeight);//加热区拍子上方加上移动拍子的安全高度 webSocketService.pushCraftsDebug(CraftsDebugGenerator.generateJson(heatId, "将机械臂移动至拍子上方加上移动拍子的安全高度", heatAreaCapClawSafetyHeightPoint3D)); 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 70fee8c..9a7574f 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,8 @@ 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.service.GDDeviceStatusService; +import com.iflytop.gd.hardware.type.IO.InputIOMId; import com.iflytop.gd.hardware.type.LiquidDistributionArmDriver; import com.iflytop.gd.hardware.type.Servo.LiquidArmMId; import lombok.RequiredArgsConstructor; @@ -25,6 +27,7 @@ public class DeviceCommandUtilService { private final DeviceCommandService deviceCommandService; private final DevicePositionService devicePositionService; private final LiquidDistributionArmDriver liquidDistributionArmDriver; + private final GDDeviceStatusService gdDeviceStatusService; /** * 门电机移动 @@ -231,14 +234,38 @@ public class DeviceCommandUtilService { } /** + * 将拍子升降区抬升至可以获取的位置 + */ + public void capUpBalance() throws Exception { + capUpBalance(null, null); + } + + /** + * 将拍子升降区抬升至可以获取的位置 + */ + public void capUpBalance(String cmdId, String cmdCode) throws Exception { + int capNum = 0; + for (InputIOMId inputIOMId : InputIOMId.values()) { + Boolean exist = gdDeviceStatusService.getInputState(inputIOMId); + if (exist) { + capNum++; + } + } + double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance(); + double capLiftingHeightSum = (6 - capNum) * capLiftingHeight; + DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(capLiftingHeightSum); + CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand); + commandWait(deviceCommandFuture); + } + + /** * 调整拍子升降电机位置 * * @param num 1为上升1格，-1为下降1格 */ public void capMotorMove(String cmdId, String cmdCode, int num) throws Exception { - double capHeight = 0.0; //TODO 每个拍子的高度 从数据库中获取 - //TODO 获取当前拍子升降电机位置 - DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(num * capHeight); + double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance(); + DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(num * capLiftingHeight); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand); commandWait(deviceCommandFuture); } diff --git a/src/main/java/com/iflytop/gd/common/enums/data/DevicePositionCode.java b/src/main/java/com/iflytop/gd/common/enums/data/DevicePositionCode.java index a7b0d21..080223e 100644 --- a/src/main/java/com/iflytop/gd/common/enums/data/DevicePositionCode.java +++ b/src/main/java/com/iflytop/gd/common/enums/data/DevicePositionCode.java @@ -83,6 +83,11 @@ public enum DevicePositionCode { */ testTubeSafetyHeight(DevicePositionType.DISTANCE, "移动试管安全高度"), + /** + * 拍子升降高度 + */ + capLiftingHeight(DevicePositionType.DISTANCE, "拍子升降高度"), + ////////////////////////点 /**