Browse Source

暂时去掉移动避让

master
白凤吉 2 months ago
parent
commit
31e2abf2a1
  1. 8
      src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java
  2. 16
      src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java
  3. 41
      src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java
  4. 2
      src/main/java/com/iflytop/gd/app/ws/server/WebSocketSender.java

8
src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java

@ -90,7 +90,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
// Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
@ -105,7 +105,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(0);//加液模块是否有托盘
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
// gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);Thread.sleep(3200);//TODO 结构有问题临时避让 完毕可以升起了顺带提升目标加热模块
gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使托盘落入加热模块
@ -114,14 +114,14 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
trayState.setInHeatModule(true);
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开准备夹取拍子
gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹爪落入拍子升降模块拍子孔位
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧夹住拍子
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方
capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);Thread.sleep(2500);//TODO 结构有问题临时避让 完毕可以升起了顺带提升目标加热模块
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);Thread.sleep(2500);//TODO 结构有问题临时避让 完毕可以升起了顺带提升目标加热模块
gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹拍子落入加热模块
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开释放拍子
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(true);//加热模块是否存在拍子

16
src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java

@ -15,6 +15,7 @@ import com.iflytop.gd.app.service.device.module.HeatModuleService;
import com.iflytop.gd.app.service.device.module.SolutionModuleService;
import com.iflytop.gd.common.annotation.CommandMapping;
import com.iflytop.gd.common.enums.HeatModuleCode;
import com.iflytop.gd.common.enums.automaton.CraftStates;
import com.iflytop.gd.common.enums.data.DevicePositionCode;
import com.iflytop.gd.common.exception.AppException;
import com.iflytop.gd.common.result.ResultCode;
@ -73,7 +74,10 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
throw new AppException(ResultCode.SOLUTION_MODULE_OCCUPIED);
}
if (trayState != null && trayState.getCrafts() != null) {
throw new AppException(ResultCode.CRAFT_RUNNING);
CraftStates craftStates = trayState.getCrafts().getState();
if(!(CraftStates.ERROR.equals(craftStates) || CraftStates.STOPPED.equals(craftStates))){
throw new AppException(ResultCode.CRAFT_RUNNING);
}
}
} catch (Exception e) {
deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false);
@ -93,7 +97,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
// Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
@ -109,10 +113,10 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧夹住拍子
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子
capModuleService.capMotorMoveByNumNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), -1);//拍子存放模块下降1个拍子位置
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0); Thread.sleep(2000);//抬升z轴
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 临时避 恢复抬起状态
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 临时避 恢复抬起状态
gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开释放夹取的拍子
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
@ -125,7 +129,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
trayState.setInHeatModule(false);
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//TODO 临时避让下降
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);Thread.sleep(2500);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
// gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(null, heatModuleCode);//TODO 临时避 恢复抬起状态
gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块

41
src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java

@ -89,8 +89,10 @@ public class CraftsStepService {
Double temperature = deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).getTemperature();
if (temperature > targetTemperature) {
heatModuleService.fanStart(heatModuleCode);
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setHeatingType(HeatingType.cooling);
} else {
heatModuleService.heatRodOpen(heatModuleCode, targetTemperature);
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setHeatingType(HeatingType.warm_up);
}
}
}
@ -119,19 +121,20 @@ public class CraftsStepService {
&& deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).getTrayStatus() == 1) {
moveToSolutionModule(heatModuleCode);
}
JSONArray addLiquidList = params.getJSONArray("list");
JSONArray list = params.getJSONArray("list");
TrayState trayState = deviceStateService.getDeviceState().getTrayStateByHeatModuleCode(heatModuleCode);
TubeState[] tubeArray = trayState.getTubes();
for (int i = 0; i < addLiquidList.size(); i++) {
JSONObject addLiquidObject = addLiquidList.getJSONObject(i);
int tubeNum = addLiquidObject.getInt("tubeNum");
for (int i = 0; i < list.size(); i++) {
JSONObject listItem = list.getJSONObject(i);
int tubeNum = listItem.getInt("tubeNum");
TubeState tubeState = tubeArray[tubeNum - 1];
if (tubeState.isNeedAddSolution()) {
log.info("工艺{},移动加液机械臂到指定试管({})", heatModuleCode, tubeNum);
solutionModuleService.dualRobotMovePoint(tubeNum);//移动加液机械臂到指定试管
JSONArray solutionList = listItem.getJSONArray("solutionList");
List<CommandFuture> futuresList = new ArrayList<>();
for (int j = 0; j < addLiquidList.size(); j++) {
JSONObject addLiquid = addLiquidList.getJSONObject(j);
for (int j = 0; j < solutionList.size(); j++) {
JSONObject addLiquid = solutionList.getJSONObject(j);
Long solutionId = addLiquid.getLong("solutionId");
Double volume = addLiquid.getDouble("volume");
Double offset = addLiquid.getDouble("offset");
@ -202,7 +205,7 @@ public class CraftsStepService {
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setHeatingType(HeatingType.warm_up);
log.info("工艺{},等待升温", heatModuleCode);
if (inTemperature != null) {
while (deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).getTemperature() + 10 < inTemperature) {
while (deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).getTemperature() + 1 < inTemperature) {
delay(1);
}
log.info("工艺{},温度达到可以放入的温度,下降加热模块托盘", heatModuleCode);
@ -228,8 +231,11 @@ public class CraftsStepService {
while (LocalDateTime.now().isBefore(endHeatTime)) {
delay(1);
}
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setStartHeatTime(null);
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTargetTime(null);
log.info("工艺{},加热完成", heatModuleCode);
log.info("工艺{},抬升加热位托盘", heatModuleCode);
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//加热模块托盘升降状态
heatModuleService.heaterMotorMove(heatModuleCode, trayLift);//抬升加热位托盘
delay(coolingSecond);
return true;
@ -280,7 +286,7 @@ public class CraftsStepService {
Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
// Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
@ -296,12 +302,12 @@ public class CraftsStepService {
log.info("工艺{},将夹爪收紧,夹住托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayGrip);//将夹爪收紧夹住托盘
log.info("工艺{},抬升z轴", heatModuleCode);
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
// 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号加热模块上方
// gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
log.info("工艺{},将携带托盘的机械臂移动至加热模块上方", heatModuleCode);
gantryModuleService.gantryMove(heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
log.info("工艺{},临时避让完毕,升起", heatModuleCode);
@ -319,7 +325,7 @@ public class CraftsStepService {
log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode);
gantryModuleService.gantryMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
log.info("工艺{},将夹爪打开,准备夹取拍子", heatModuleCode);
gantryModuleService.clawMove(clawCapPick);//将夹爪打开准备夹取拍子
log.info("工艺{},下降z轴,使夹爪落入拍子升降模块拍子孔位", heatModuleCode);
@ -332,8 +338,8 @@ public class CraftsStepService {
gantryModuleService.gantryMove(heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方
log.info("工艺{},提升拍子存放区", heatModuleCode);
capModuleService.capUpBalanceNoWait(); //提升拍子存放区
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode);
Thread.sleep(4000);//TODO 结构有问题临时避让 完毕可以升起了顺带提升目标加热模块
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode);
// Thread.sleep(4000);//TODO 结构有问题临时避让 完毕可以升起了顺带提升目标加热模块
log.info("工艺{},下降z轴,使夹拍子落入加热模块", heatModuleCode);
gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹拍子落入加热模块
log.info("工艺{},将夹爪打开,释放拍子", heatModuleCode);
@ -364,7 +370,7 @@ public class CraftsStepService {
Point3D liquidAreaTrayPoint3D = devicePositionService.getPosition(DevicePositionCode.liquidAreaTrayPoint).getPoint3D();//获取加液模块上方点位
Point3D heatAreaTrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModuleCode);//获取指定加热模块上方点位
Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
// Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);//获取指定加热模块上方点位
Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
Point3D capStorageCapClawPoint3D = devicePositionService.getPosition(DevicePositionCode.capStorageCapClawPoint).getPoint3D();//获取拍子存放区上方点位;
@ -388,13 +394,12 @@ public class CraftsStepService {
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子
log.info("工艺{},拍子存放模块下降1个拍子位置", heatModuleCode);
capModuleService.capMotorMoveByNumNoWait(-1);//拍子存放模块下降1个拍子位置
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);
Thread.sleep(4000);//抬升z轴
log.info("工艺{},将携带拍子的机械臂移动至存放区上方", heatModuleCode);
gantryModuleService.gantryMove(capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(null);//TODO 临时避 恢复抬起状态
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(null);//TODO 临时避 恢复抬起状态
log.info("工艺{},下降z轴,使夹拍子落入存放区", heatModuleCode);
gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区
log.info("工艺{},将夹爪打开,释放夹取的拍子", heatModuleCode);
@ -418,7 +423,7 @@ public class CraftsStepService {
gantryModuleService.gantryZMove(0);
Thread.sleep(2500);//抬升z轴
log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode);
gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
// gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
log.info("工艺{},将携带托盘的机械臂移动至加液模块上方", heatModuleCode);
gantryModuleService.gantryMove(liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
log.info("工艺{},临时避 恢复抬起状态", heatModuleCode);

2
src/main/java/com/iflytop/gd/app/ws/server/WebSocketSender.java

@ -16,7 +16,7 @@ public class WebSocketSender {
websocketResult.setType(type);
websocketResult.setData(data);
WebSocketServer.sendMessageToClients(JSONUtil.toJsonStr(websocketResult));
log.info("WS::{}", JSONUtil.toJsonStr(websocketResult));
// log.info("WS::{}", JSONUtil.toJsonStr(websocketResult));
}
public void pushCraftsDebug(Object data) {

Loading…
Cancel
Save