graphite-digester-service/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java

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.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 AtomicBoolean isExecuting = new AtomicBoolean(false);

    @Override
    public CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception {
        if (isExecuting.get()) {
            throw new AppException(ResultCode.COMMAND_ALREADY_EXECUTING);
        }
        String heatId = cmdDTO.getStringParam("heatId");
        HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId);
        //校验目标加热位是否有托盘
        try{
            deviceStateService.setGantryArmStateIdle(true);
            isExecuting.set(true);
            Boolean heatModuleTray = deviceCommandUtilService.heatModuleTray(heatModuleId);
            if (heatModuleTray) {
                throw new AppException(ResultCode.TARGET_HEAT_MODULE_OCCUPIED);
            }
        }finally {
            deviceStateService.setGantryArmStateIdle(false);
            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 {
                deviceStateService.setGantryArmStateIdle(false);
                isExecuting.set(false);
            }
        });
    }
}