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

feat:工艺避让移动

master
白凤吉 2 months ago
parent
9a3057e2dc
commit
b1946d4d84
2 changed files with 37 additions and 156 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. 178
      src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java
  2. 15
      src/main/java/com/iflytop/gd/app/service/device/ActionCommandService.java

178
src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java
View File

@ -16,6 +16,7 @@ import com.iflytop.gd.app.model.entity.Photos;
import com.iflytop.gd.app.service.api.ContainerService;
import com.iflytop.gd.app.service.api.DevicePositionService;
import com.iflytop.gd.app.service.api.PhotosService;
import com.iflytop.gd.app.service.device.ActionCommandService;
import com.iflytop.gd.app.service.device.DeviceCommandTempUtilService;
import com.iflytop.gd.app.service.device.DeviceStateService;
import com.iflytop.gd.app.service.device.module.CapModuleService;
@ -69,6 +70,7 @@ public class CraftsStepService {
private final DeviceCommandTempUtilService deviceCommandTempUtilService;
private final CameraBaslerDriver cameraBaslerDriver;
private final PhotosService photosService;
private final ActionCommandService actionCommandService;
@Value("${photo.url}")
private String url;
@ -396,170 +398,34 @@ public class CraftsStepService {
* 将托盘移至加热
*/
private void moveToHeatModule(HeatModuleCode heatModuleCode) throws Exception {
double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //获取拍子夹取距离
double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离
double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离
double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离
double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //托盘升降抬升距离
double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
double heatModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleCapMoveHeight).getDistance();//加热模块拍子移动高度
double heatModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleTrayMoveHeight).getDistance();//加热模块托盘z轴移动高度
double solutionModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.solutionModuleTrayMoveHeight).getDistance();//加液模块托盘z轴移动高度
double capModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.capModuleCapMoveHeight).getDistance();//拍子升降模块拍子z轴移动高度
Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
// Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
TrayState trayState = deviceStateService.getDeviceState().getTrayInSolutionModule();
log.info("工艺{},提升拍子存放区至拍子夹取的高度", heatModuleCode);
capModuleService.capUpBalanceNoWait();//提升拍子存放区至拍子夹取的高度
log.info("工艺{},将机械臂移动至加液模块上方", heatModuleCode);
gantryModuleService.gantryXYMove(liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方
log.info("工艺{},将夹爪打开，准备夹取托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，准备夹取托盘
log.info("工艺{},下降z轴,使夹爪落入托盘孔位", heatModuleCode);
gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
log.info("工艺{},将夹爪收紧，夹住托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayGrip);//将夹爪收紧，夹住托盘
log.info("工艺{},抬升z轴", heatModuleCode);
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块
gantryModuleService.gantryZMove(0);//抬升z轴
deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(0);//加液模块是否有托盘
deviceStateService.getDeviceState().getTrayStateByHeatModuleCode(heatModuleCode).setInSolutionModule(false);
log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode);
// gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
log.info("工艺{},将携带托盘的机械臂移动至加热模块上方", heatModuleCode);
gantryModuleService.gantryXYMove(heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
log.info("工艺{},临时避让完毕，升起", heatModuleCode);
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块
Thread.sleep(3200);
log.info("工艺{},下降z轴,使托盘落入加热模块", heatModuleCode);
gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使托盘落入加热模块
log.info("工艺{},将夹爪打开，释放托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，释放托盘
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(1);//加热模块是否存在托盘
deviceStateService.getDeviceState().getTrayStateByHeatModuleCode(heatModuleCode).setInHeatModule(true);
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode);
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块
log.info("工艺{},将夹爪打开，准备夹取拍子", heatModuleCode);
gantryModuleService.clawMove(clawCapPick);//将夹爪打开，准备夹取拍子
log.info("工艺{},下降z轴,使夹爪落入拍子升降模块拍子孔位", heatModuleCode);
gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹爪落入拍子升降模块拍子孔位
log.info("工艺{},将夹爪收紧，夹住拍子", heatModuleCode);
gantryModuleService.clawMove(clawCapGrip);//将夹爪收紧，夹住拍子
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},将携带拍子的机械臂移动至加热模块拍子上方", heatModuleCode);
gantryModuleService.gantryXYMove(heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方
log.info("工艺{},提升拍子存放区", heatModuleCode);
capModuleService.capUpBalanceNoWait(); //提升拍子存放区
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode);
// Thread.sleep(4000);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块
log.info("工艺{},下降z轴,使夹拍子落入加热模块", heatModuleCode);
gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹拍子落入加热模块
log.info("工艺{},将夹爪打开，释放拍子", heatModuleCode);
gantryModuleService.clawMove(clawCapPick);//将夹爪打开，释放拍子
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(true);//加热模块是否存在拍子
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode);
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
log.info("工艺{},释放加液区", heatModuleCode);
solutionModuleService.releaseSolutionModule();//释放加液区
try {
capModuleService.capUpBalanceNoWait();//提升拍子存放区至拍子夹取的高度
actionCommandService.moveTrayToHeatModule(heatModuleCode);//将托盘移动至指定加热模块
actionCommandService.heatModuleInstallCap(heatModuleCode);//指定加热模块安装拍子
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
} finally {
deviceStateService.getDeviceState().getGantryArm().setIdle(true);//机械臂空闲
deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false);//该指令结束
solutionModuleService.releaseSolutionModule();//释放加液区等待
}
}
/**
* 将托盘移至加液
*/
private void moveToSolutionModule(HeatModuleCode heatModuleCode) throws Exception {
double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //获取拍子夹取距离
double clawCapGrip = devicePositionService.getPosition(DevicePositionCode.clawCapGrip).getDistance(); //获取拍子夹紧距离
double clawTrayPick = devicePositionService.getPosition(DevicePositionCode.clawTrayPick).getDistance();//获取夹爪托盘夹取距离
double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();//获取夹爪托盘夹紧距离
double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //托盘升降抬升距离
double trayLower = devicePositionService.getPosition(DevicePositionCode.trayLower).getDistance(); //获取加热位下降托盘位置
double heatModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleCapMoveHeight).getDistance();//加热模块拍子移动高度
double heatModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleTrayMoveHeight).getDistance();//加热模块托盘z轴移动高度
double solutionModuleTrayMoveHeight = devicePositionService.getPosition(DevicePositionCode.solutionModuleTrayMoveHeight).getDistance();//加液模块托盘z轴移动高度
double capModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.capModuleCapMoveHeight).getDistance();//拍子升降模块拍子z轴移动高度
Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
// Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
TrayState trayState = deviceStateService.getDeviceState().getTrayStateByHeatModuleCode(heatModuleCode);
log.info("工艺{},申请使用加液区并等待", heatModuleCode);
solutionModuleService.requestSolutionModule();//申请使用加液区并等待
log.info("工艺{},提升拍子存放区至拍子夹取的高度", heatModuleCode);
capModuleService.capUpBalanceNoWait(); //提升拍子存放区至拍子夹取的高度
log.info("工艺{},将机械臂移动至加热模块拍子上方", heatModuleCode);
gantryModuleService.gantryXYMove(heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起
log.info("工艺{},抬升指定加热位托盘", heatModuleCode);
heatModuleService.heaterMotorMove(heatModuleCode, trayLift);//抬升指定加热位托盘
log.info("工艺{},将夹爪打开，准备夹取拍子", heatModuleCode);
gantryModuleService.clawMove(clawCapPick);//将夹爪打开，准备夹取拍子
log.info("工艺{},下降z轴,使夹爪落入加热模块拍子孔位", heatModuleCode);
gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位
log.info("工艺{},将夹爪收紧，夹住拍子", heatModuleCode);
gantryModuleService.clawMove(clawCapGrip);//将夹爪收紧，夹住拍子
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子
log.info("工艺{},拍子存放模块下降1个拍子位置", heatModuleCode);
capModuleService.capMotorMoveByNumNoWait(-1);//拍子存放模块下降1个拍子位置
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);
log.info("工艺{},将携带拍子的机械臂移动至存放区上方", heatModuleCode);
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(null);//TODO 临时避 恢复抬起状态
log.info("工艺{},下降z轴,使夹拍子落入存放区", heatModuleCode);
gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区
log.info("工艺{},将夹爪打开，释放夹取的拍子", heatModuleCode);
gantryModuleService.clawMove(clawCapPick);//将夹爪打开，释放夹取的拍子
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},将机械臂移动至加热模块上方", heatModuleCode);
gantryModuleService.gantryXYMove(heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
log.info("工艺{},将夹爪打开，准备夹取托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，准备夹取托盘
log.info("工艺{},下降z轴,使夹爪落入托盘孔位", heatModuleCode);
gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
log.info("工艺{},将夹爪收紧，夹住托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayGrip);//将夹爪收紧，夹住托盘
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(0);//加热模块是否存在托盘
trayState.setInHeatModule(false);
log.info("工艺{},临时避让下降", heatModuleCode);
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(heatModuleCode);//TODO 临时避让下降
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMoveZero();//抬升z轴
Thread.sleep(2500);//抬升z轴
log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode);
// gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
log.info("工艺{},将携带托盘的机械臂移动至加液模块上方", heatModuleCode);
gantryModuleService.gantryXYMove(liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
log.info("工艺{},临时避 恢复抬起状态", heatModuleCode);
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(null, heatModuleCode);//TODO 临时避 恢复抬起状态
log.info("工艺{},下降z轴,使托盘落入加液模块", heatModuleCode);
gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块
deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(1);//加液模块是否存在托盘
trayState.setInSolutionModule(true);
log.info("工艺{},将夹爪打开，释放托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，释放托盘
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMoveZero();//抬升z轴
log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode);
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
try {
solutionModuleService.requestSolutionModule();//申请使用加液区并等待
capModuleService.capUpBalanceNoWait(); //提升拍子存放区至拍子夹取的高度
actionCommandService.heatModuleRemoveCap(heatModuleCode);//指定加热模块移除拍子
actionCommandService.heatModuleRemoveTray(heatModuleCode);//指定加热模块移除托盘
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
} finally {
deviceStateService.getDeviceState().getGantryArm().setIdle(true);//机械臂空闲
deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false);//该指令结束
}
}
}

15
src/main/java/com/iflytop/gd/app/service/device/ActionCommandService.java
View File

@ -28,6 +28,9 @@ public class ActionCommandService {
private final GantryModuleService gantryModuleService;
private final DeviceCommandTempUtilService deviceCommandTempUtilService;
public void heatModuleRemoveTray(HeatModuleCode heatModuleCode) throws Exception {
heatModuleRemoveTray(null, null, heatModuleCode);
}
/**
* 指定加热模块移除托盘
@ -68,6 +71,10 @@ public class ActionCommandService {
gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴
}
public void heatModuleRemoveCap(HeatModuleCode heatModuleCode) throws Exception {
heatModuleRemoveCap(null, null, heatModuleCode);
}
/**
* 指定加热模块移除拍子
*/
@ -100,6 +107,10 @@ public class ActionCommandService {
gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴
}
public void heatModuleInstallCap(HeatModuleCode heatModuleCode) throws Exception {
heatModuleInstallCap(null, null, heatModuleCode);
}
/**
* 指定加热模块安装拍子
*/
@ -131,6 +142,10 @@ public class ActionCommandService {
gantryModuleService.gantryZMoveZero(commandId, command);//抬升z轴
}
public void moveTrayToHeatModule(HeatModuleCode heatModuleCode) throws Exception {
moveTrayToHeatModule(null, null, heatModuleCode);
}
/**
* 从加液模块移动托盘至指定加热模块
*/

Write Preview
Loading…
Cancel
Save