package com.iflytop.gd.app.cmd; import com.iflytop.gd.app.core.BaseCommandHandler; import com.iflytop.gd.app.core.device.TrayState; import com.iflytop.gd.app.model.bo.Point3D; import com.iflytop.gd.app.model.dto.CmdDTO; import com.iflytop.gd.app.service.DeviceCommandUtilService; import com.iflytop.gd.app.service.DevicePositionService; import com.iflytop.gd.app.service.DeviceStateService; import com.iflytop.gd.app.service.GantryArmService; import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.common.enums.data.DevicePositionCode; import com.iflytop.gd.common.exception.AppException; import com.iflytop.gd.common.result.ResultCode; import com.iflytop.gd.hardware.exception.HardwareException; import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; import java.util.concurrent.CompletableFuture; import java.util.concurrent.atomic.AtomicBoolean; /** * 移至加液 */ @Slf4j @Component @RequiredArgsConstructor @CommandMapping("move_to_solution_area")//业务指令注解 public class MoveToSolutionAreaCommand extends BaseCommandHandler { private final DeviceCommandUtilService deviceCommandUtilService; private final DevicePositionService devicePositionService; private final GantryArmService gantryArmService; private final DeviceStateService deviceStateService; private final MoveToHeatAreaCommand moveToHeatAreaCommand; @Getter private final AtomicBoolean isExecuting = new AtomicBoolean(false); @Override public CompletableFuture handle(CmdDTO cmdDTO) throws Exception { if (isExecuting.get() || moveToHeatAreaCommand.getIsExecuting().get()) { throw new AppException(ResultCode.COMMAND_ALREADY_EXECUTING); } isExecuting.set(true); String heatId = cmdDTO.getStringParam("heatId"); HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId); //校验目标加热位是否有托盘 try{ Boolean heatModuleTray = deviceCommandUtilService.heatModuleTray(heatModuleId); if (heatModuleTray) { throw new AppException(ResultCode.TARGET_HEAT_MODULE_OCCUPIED); } }finally { isExecuting.set(false); } return runAsync(() -> { try{ TrayState trayState = deviceStateService.getTrayInSolutionModule(); gantryArmService.waitLiquidIdle();//等待加液区空闲 gantryArmService.setLiquidIdleFalse();//锁定加液区 double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //获取加热位抬升托盘位置 deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLift);//抬升加热位托盘 deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 2); //TODO 判断托盘是否有拍子 Point3D heatAreaCapClawPoint3D = deviceCommandUtilService.getHeatAreaCapClawPointPoint3D(heatModuleId);//获取托盘上方拍子点位 deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPoint3D);//将机械臂移动至拍子上方 double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //拍子夹取距离 deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开 double clawDescend = devicePositionService.getPosition(DevicePositionCode.clawDescend).getDistance();//获取下降机械臂使夹爪可以夹住的距离 deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住拍子 double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离 deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧 double capSafetyHeight = devicePositionService.getPosition(DevicePositionCode.capSafetyHeight).getDistance();//获取移动拍子的安全高度 deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -capSafetyHeight);//机械臂提升至移动拍子的安全高度 deviceStateService.setHeatModuleStateCapExist(heatModuleId, false); deviceCommandUtilService.capMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), -1); //下降拍子存放区1个位置 Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位; Point3D capStorageCapClawSafetyHeightPoint3D = new Point3D(capStorageCapClawPoint3D.getX(), capStorageCapClawPoint3D.getY(),capStorageCapClawPoint3D.getZ() - capSafetyHeight);//拍子存放区上方加上移动拍子的安全高度 deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawSafetyHeightPoint3D);//移动机械臂至拍子存放区上方上减去拍子的安全高度 deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), capSafetyHeight);//下降机械臂,使拍子落入存放区 deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick); //松开夹爪,放开拍子 deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend);//将机械臂抬升至托盘上方 Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(heatModuleId);//获取指定托盘上方点位 deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至托盘上方 double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离 deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪张开 deviceCommandUtilService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawDescend); //下降机械臂使夹爪可以夹住托盘 double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离 deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧 double traySafetyHeight = devicePositionService.getPosition(DevicePositionCode.traySafetyHeight).getDistance();//获取移动托盘的安全高度 deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -traySafetyHeight);//机械臂提升至移动托盘的安全高度 deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 0); trayState.setInHeatModule(false); Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液区托盘夹爪点 Point3D liquidAreaTraySafetyHeightPoint3D = new Point3D(liquidAreaTrayPoint3D.getX(), liquidAreaTrayPoint3D.getY(),liquidAreaTrayPoint3D.getZ() - traySafetyHeight);//加液区托盘点位上方加上移动托盘的安全高度 deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTraySafetyHeightPoint3D);//移动机械臂至加液区托盘点位上方减去移动托盘的安全高度 deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight);//下降机械臂将托盘与试管落入加液区 deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//松开夹爪,放下托盘 deviceStateService.setSolutionModuleStateTrayStatus(1); trayState.setInSolutionModule(true); deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//抬升机械臂至托盘上方 }finally { isExecuting.set(false); } }); } }