baifengji
/
graphite-digester-service
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

feat:增加移至加热、加液校验托盘状态

master
白凤吉 3 months ago
parent
4f38c14785
commit
bf8117e572
5 changed files with 155 additions and 96 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 112
      src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java
  2. 114
      src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java
  3. 16
      src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java
  4. 2
      src/main/java/com/iflytop/gd/common/cmd/CommandHandler.java
  5. 7
      src/main/java/com/iflytop/gd/common/result/ResultCode.java

112
src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java
View File

@ -11,11 +11,16 @@ 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;
/**
* 移至加热
@ -29,59 +34,72 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
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) {
public CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception {
if (isExecuting.get()) {
throw new AppException(ResultCode.COMMAND_ALREADY_EXECUTING);
}
isExecuting.set(true);
String heatId = cmdDTO.getStringParam("heatId");
HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId);
//TODO 需要校验目标加热位是否有托盘，加液位置是否有托盘
//TODO 有的指令不能并行，前端也应该友好提示，机械臂是否繁忙
//TODO 简单的前端确认弹窗是否有托盘即可
//校验目标加热位是否有托盘
try{
Boolean heatModuleTray = deviceCommandUtilService.heatModuleTray(heatModuleId);
if (!heatModuleTray) {
throw new AppException(ResultCode.TARGET_HEAT_MODULE_NO_TRAY);
}
}finally {
isExecuting.set(false);
}
return runAsync(() -> {
TrayState trayState = deviceStateService.getTrayInSolutionModule();
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);
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 {
isExecuting.set(false);
}
});
}
}

114
src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java
View File

@ -11,11 +11,15 @@ 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;
/**
* 移至加液
@ -29,59 +33,75 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
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) {
public CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception {
if (isExecuting.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(() -> {
TrayState trayState = deviceStateService.getTrayInSolutionModule();
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);//抬升机械臂至托盘上方
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);
}
});
}
}

16
src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java
View File

@ -10,6 +10,7 @@ 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.exception.HardwareException;
import com.iflytop.gd.hardware.service.GDDeviceStatusService;
import com.iflytop.gd.hardware.type.IO.InputIOMId;
import com.iflytop.gd.hardware.type.LiquidDistributionArmDriver;
@ -549,6 +550,21 @@ public class DeviceCommandUtilService {
commandWait(deviceCommandFuture);
}
/**
* 获取指定加热模块是否有托盘
*/
public Boolean heatModuleTray(HeatModuleCode heatModuleCode) throws HardwareException {
return switch (heatModuleCode) {
case heat_module_01 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE1_EXSIT);
case heat_module_02 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE2_EXSIT);
case heat_module_03 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE3_EXSIT);
case heat_module_04 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE4_EXSIT);
case heat_module_05 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE5_EXSIT);
case heat_module_06 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE6_EXSIT);
};
}
public void commandWait(CommandFuture... futures) throws Exception {
CompletableFuture<?>[] responseFutures = Arrays.stream(futures)

2
src/main/java/com/iflytop/gd/common/cmd/CommandHandler.java
View File

@ -5,5 +5,5 @@ import com.iflytop.gd.app.model.dto.CmdDTO;
import java.util.concurrent.CompletableFuture;
public interface CommandHandler {
CompletableFuture<Void> handle(CmdDTO cmdDTO);
CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception;
}

7
src/main/java/com/iflytop/gd/common/result/ResultCode.java
View File

@ -47,7 +47,12 @@ public enum ResultCode implements IResultCode, Serializable {
COMMAND_EXEC_TIMEOUT("5003", "命令执行超时"),
HARDWARE_ERROR("5004", "硬件错误"),
//============================ 6xxx：指令相关 ============================
COMMAND_NOT_FOUND("6000", "指令未找到");
COMMAND_NOT_FOUND("6000", "指令未找到"),
COMMAND_ALREADY_EXECUTING("6001", "指令正在执行，无法重复执行"),
SENSOR_STATUS_FAILED("6010", "获取传感器状态失败"),
TARGET_HEAT_MODULE_OCCUPIED("6021", "目标加热模块被占用"),
TARGET_HEAT_MODULE_NO_TRAY("6022", "目标加热模块无托盘");
/** 状态码 */
private final String code;
/** 提示信息 */

Write Preview
Loading…
Cancel
Save