You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
109 lines
8.2 KiB
109 lines
8.2 KiB
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.service.GDDeviceStatusService;
|
|
import com.iflytop.gd.hardware.type.IO.InputIOMId;
|
|
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_heat_area")//业务指令注解
|
|
public class MoveToHeatAreaCommand extends BaseCommandHandler {
|
|
private final DeviceCommandUtilService deviceCommandUtilService;
|
|
private final DevicePositionService devicePositionService;
|
|
private final GantryArmService gantryArmService;
|
|
private final DeviceStateService deviceStateService;
|
|
private final AtomicBoolean isExecuting = new AtomicBoolean(false);
|
|
|
|
@Override
|
|
public CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception {
|
|
if (isExecuting.get()) {
|
|
throw new AppException(ResultCode.COMMAND_ALREADY_EXECUTING);
|
|
}
|
|
deviceStateService.setGantryArmStateIdle(true);
|
|
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_NO_TRAY);
|
|
}
|
|
}finally {
|
|
deviceStateService.setGantryArmStateIdle(false);
|
|
isExecuting.set(false);
|
|
}
|
|
return runAsync(() -> {
|
|
try {
|
|
TrayState trayState = deviceStateService.getTrayInSolutionModule();
|
|
|
|
Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液区托盘夹爪点
|
|
deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将机械臂移动至加液区托盘上方
|
|
double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离
|
|
deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪张开
|
|
double clawDescend = devicePositionService.getPosition(DevicePositionCode.clawDescend).getDistance();//获取下降机械臂使夹爪可以夹住的距离
|
|
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.setSolutionModuleStateTrayStatus(0);
|
|
Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(heatModuleId);//获取指定托盘上方点位
|
|
Point3D heatAreaTrayClawSafetyHeightPoint3D = new Point3D(heatAreaTrayClawPoint3D.getX(), heatAreaTrayClawPoint3D.getY(), heatAreaTrayClawPoint3D.getZ() - traySafetyHeight);//加热区托盘点位上方减去移动托盘的安全高度
|
|
deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawSafetyHeightPoint3D);//将携带托盘的机械臂移动至托盘上方
|
|
deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight);//下降机械臂将托盘与试管落入加热区
|
|
deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//将机械臂提升至托盘上方高度
|
|
Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
|
|
deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
|
|
double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //拍子夹取距离
|
|
deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开
|
|
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);//机械臂提升至移动拍子的安全高度
|
|
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);//将机械臂移动至拍子上方加上移动拍子的安全高度
|
|
deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), traySafetyHeight); //降下机械臂使拍子盖住托盘试管
|
|
trayState.setHeatModuleId(heatModuleId);
|
|
trayState.setInHeatModule(true);
|
|
deviceStateService.setHeatModuleStateCapExist(heatModuleId, true);
|
|
deviceCommandUtilService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪张开
|
|
deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//抬升机械臂至托盘上方
|
|
gantryArmService.setLiquidIdleTrue();//释放加液区等待
|
|
deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 2);
|
|
double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
|
|
deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLower);//下降加热位托盘
|
|
deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 1);
|
|
} finally {
|
|
deviceStateService.setGantryArmStateIdle(false);
|
|
isExecuting.set(false);
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|