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.
77 lines
3.8 KiB
77 lines
3.8 KiB
package com.iflytop.sgs.app.cmd;
|
|
|
|
import com.iflytop.sgs.app.core.BaseCommandHandler;
|
|
import com.iflytop.sgs.app.model.bo.Point3D;
|
|
import com.iflytop.sgs.app.model.dto.CmdDTO;
|
|
import com.iflytop.sgs.app.service.DeviceCommandUtilService;
|
|
import com.iflytop.sgs.app.service.DevicePositionService;
|
|
import com.iflytop.sgs.app.service.DeviceStateService;
|
|
import com.iflytop.sgs.app.service.GantryArmService;
|
|
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;
|
|
|
|
import java.util.concurrent.CompletableFuture;
|
|
|
|
/**
|
|
* 移至加热 todo wmy 需要改
|
|
*/
|
|
@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;
|
|
|
|
@Override
|
|
public CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception {
|
|
if (deviceStateService.getCommandState().get().isMoveToHeatAreaCommandExecuting()) {
|
|
throw new AppException(ResultCode.COMMAND_ALREADY_EXECUTING);
|
|
}
|
|
if (deviceStateService.getCommandState().get().isMoveToSolutionAreaCommandExecuting()) {
|
|
throw new AppException(ResultCode.CMD_BUSY);
|
|
}
|
|
try {
|
|
deviceStateService.getCommandState().get().setMoveToHeatAreaCommandExecuting(true);
|
|
} catch (Exception e) {
|
|
deviceStateService.getCommandState().get().setMoveToHeatAreaCommandExecuting(false);
|
|
throw e;
|
|
}
|
|
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();
|
|
//获取指定加热模块上方点位
|
|
Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(heatModuleId);
|
|
return runAsync(() -> {
|
|
try {
|
|
//升高加液电机高度
|
|
deviceCommandUtilService.motorLiquidMove(cmdDTO.getCommandId(),cmdDTO.getCommand(),liquidMotorSafeDistance);
|
|
//移动机械臂到加热区上方 此时机械臂夹着托盘
|
|
deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D); //将机械臂移动至加热模块上方
|
|
//下降z轴,夹爪脱离机械臂
|
|
deviceCommandUtilService.gantryZMove(clawTrayHeight);
|
|
//移动x轴向右脱离托盘
|
|
deviceCommandUtilService.gantryXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawTrayGrip);
|
|
//z轴返回原点
|
|
deviceCommandUtilService.motorLiquidMove(0);
|
|
} finally {
|
|
deviceStateService.setGantryArmStateIdle(true);
|
|
deviceStateService.getCommandState().get().setMoveToHeatAreaCommandExecuting(false);
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|