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

feat:增加多点折线移动

master
白凤吉 2 months ago
parent
73f72e95d2
commit
9c48064d59
11 changed files with 93 additions and 61 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. 11
      src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java
  2. 11
      src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java
  3. 2
      src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaCapCommand.java
  4. 2
      src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaTrayCommand.java
  5. 11
      src/main/java/com/iflytop/gd/app/command/debug/step/DebugShowSmogCommand.java
  6. 14
      src/main/java/com/iflytop/gd/app/command/selftest/TrayOutCommand.java
  7. 7
      src/main/java/com/iflytop/gd/app/controller/TestController.java
  8. 29
      src/main/java/com/iflytop/gd/app/service/api/TestService.java
  9. 20
      src/main/java/com/iflytop/gd/app/service/crafts/CraftsStepService.java
  10. 2
      src/main/java/com/iflytop/gd/app/service/device/DeviceStepCommandService.java
  11. 45
      src/main/java/com/iflytop/gd/app/service/device/module/GantryModuleService.java

11
src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java
View File

@ -3,7 +3,6 @@ package com.iflytop.gd.app.command.control;
import com.iflytop.gd.app.core.BaseCommandHandler;
import com.iflytop.gd.app.model.bo.Point3D;
import com.iflytop.gd.app.model.bo.status.device.HeatModuleState;
import com.iflytop.gd.app.model.bo.status.device.SolutionModuleState;
import com.iflytop.gd.app.model.bo.status.device.TrayState;
import com.iflytop.gd.app.model.dto.CmdDTO;
import com.iflytop.gd.app.service.api.DevicePositionService;
@ -100,7 +99,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
trayState.setHeatModuleId(heatModuleCode);
}
capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand());//提升拍子存放区至拍子夹取的高度
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，准备夹取托盘
gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧，夹住托盘
@ -108,7 +107,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
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(), heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);Thread.sleep(3200);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块
gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使托盘落入加热模块
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，释放托盘
@ -117,20 +116,20 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
trayState.setInHeatModule(true);
}
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
// 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);//将携带拍子的机械臂移动至加热模块拍子上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方
capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区
// 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);//加热模块是否存在拍子
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
} finally {
deviceStateService.getDeviceState().getGantryArm().setIdle(true);
deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false);

11
src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java
View File

@ -2,7 +2,6 @@ package com.iflytop.gd.app.command.control;
import com.iflytop.gd.app.core.BaseCommandHandler;
import com.iflytop.gd.app.model.bo.Point3D;
import com.iflytop.gd.app.model.bo.status.device.HeatModuleState;
import com.iflytop.gd.app.model.bo.status.device.SolutionModuleState;
import com.iflytop.gd.app.model.bo.status.device.TrayState;
import com.iflytop.gd.app.model.dto.CmdDTO;
@ -105,7 +104,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
try {
solutionModuleService.requestSolutionModule();//申请使用加液区并等待
capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区至拍子夹取的高度
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起
heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLift);//抬升指定加热位托盘
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，准备夹取拍子
@ -115,13 +114,13 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
capModuleService.capMotorMoveByNumNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), -1);//拍子存放模块下降1个拍子位置
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
// 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轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，准备夹取托盘
gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧，夹住托盘
@ -132,7 +131,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
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(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(null, heatModuleCode);//TODO 临时避 恢复抬起状态
gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块
deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(1);//加液模块是否存在托盘
@ -141,7 +140,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
}
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，释放托盘
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
} finally {
deviceStateService.getDeviceState().getGantryArm().setIdle(true);
deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false);

2
src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaCapCommand.java
View File

@ -31,7 +31,7 @@ public class DebugGantryToHeatAreaCapCommand extends BaseCommandHandler {
Point3D point3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModule);
point3D.setZ((double) 0);
return runAsync(() -> {
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), point3D);//将携带拍子的机械臂移动至加热模块拍子上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), point3D);//将携带拍子的机械臂移动至加热模块拍子上方
});
}
}

2
src/main/java/com/iflytop/gd/app/command/debug/step/DebugGantryToHeatAreaTrayCommand.java
View File

@ -31,7 +31,7 @@ public class DebugGantryToHeatAreaTrayCommand extends BaseCommandHandler {
Point3D point3D = heatModuleService.getHeatAreaTrayClawPoint3D(heatModule);
point3D.setZ((double) 0);
return runAsync(() -> {
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), point3D);
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), point3D);
});
}
}

11
src/main/java/com/iflytop/gd/app/command/debug/step/DebugShowSmogCommand.java
View File

@ -4,20 +4,11 @@ import com.iflytop.gd.app.core.BaseCommandHandler;
import com.iflytop.gd.app.model.bo.Point3D;
import com.iflytop.gd.app.model.dto.CmdDTO;
import com.iflytop.gd.app.service.api.DevicePositionService;
import com.iflytop.gd.app.service.device.DeviceCommandService;
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;
import com.iflytop.gd.app.service.device.module.GantryModuleService;
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.command.CommandFuture;
import com.iflytop.gd.common.command.DeviceCommandBundle;
import com.iflytop.gd.common.command.DeviceCommandGenerator;
import com.iflytop.gd.common.enums.HeatModuleCode;
import com.iflytop.gd.common.enums.data.DevicePositionCode;
import com.iflytop.gd.common.utils.CommandUtil;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;
@ -47,7 +38,7 @@ public class DebugShowSmogCommand extends BaseCommandHandler {
double heatModuleCapMoveHeight = devicePositionService.getPosition(DevicePositionCode.heatModuleCapMoveHeight).getDistance();//加热模块拍子移动高度
Point3D heatAreaCapClawPointPoint3D = heatModuleService.getHeatAreaCapClawPointPoint3D(heatModuleCode);//获取指定加热模块拍子上方点位
return runAsync(() -> {
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLift);//抬升指定加热位托盘
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，准备夹取拍子
gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位

14
src/main/java/com/iflytop/gd/app/command/selftest/TrayOutCommand.java
View File

@ -6,12 +6,10 @@ import com.iflytop.gd.app.model.dto.CmdDTO;
import com.iflytop.gd.app.service.api.DevicePositionService;
import com.iflytop.gd.app.service.device.DeviceCommandTempUtilService;
import com.iflytop.gd.app.service.device.DeviceStateService;
import com.iflytop.gd.app.service.device.SelfTestService;
import com.iflytop.gd.app.service.device.module.*;
import com.iflytop.gd.common.annotation.CommandMapping;
import com.iflytop.gd.common.enums.HeatModuleCode;
import com.iflytop.gd.common.enums.data.DevicePositionCode;
import io.swagger.v3.oas.annotations.media.Schema;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;
@ -66,7 +64,7 @@ public class TrayOutCommand extends BaseCommandHandler {
// TrayState trayState = deviceStateService.getTrayInSolutionModule();
solutionModuleService.requestSolutionModule();//申请使用加液区并等待
capModuleService.capUpBalance(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区至拍子夹取的高度
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLift);//抬升指定加热位托盘
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，准备夹取拍子
gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位
@ -74,12 +72,12 @@ public class TrayOutCommand extends BaseCommandHandler {
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
capModuleService.capMotorMoveByNum(-1);//拍子存放模块下降1个拍子位置
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开，释放夹取的拍子
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，准备夹取托盘
gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
@ -90,15 +88,15 @@ public class TrayOutCommand extends BaseCommandHandler {
// deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLower);//下降加热模块托盘
// deviceStateService.setHeatModuleStateTrayUp(heatModuleId, 0);//加热模块托盘升降状态
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDown(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//TODO 临时避让下降
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDown(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 临时避 恢复抬起状态
gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块
// trayState.setInSolutionModule(true);//托盘是否在加液模块中
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(1);//加液模块是否存在托盘
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开，释放托盘
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
gantryModuleService.gantryXYMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
} finally {
deviceStateService.getDeviceState().getGantryArm().setIdle(true);
deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false);

7
src/main/java/com/iflytop/gd/app/controller/TestController.java
View File

@ -127,4 +127,11 @@ public class TestController {
testService.countPackage();
return Result.success();
}
@Operation(summary = "多点移动测试")
@PostMapping("/move-points")
public Result<List<AllSensorDTO>> moveManyPoints() throws Exception {
testService.moveManyPoints();
return Result.success();
}
}

29
src/main/java/com/iflytop/gd/app/service/api/TestService.java
View File

@ -1,8 +1,11 @@
package com.iflytop.gd.app.service.api;
import com.iflytop.gd.app.model.bo.Point3D;
import com.iflytop.gd.app.model.dto.AllSensorDTO;
import com.iflytop.gd.app.service.device.DeviceCommandService;
import com.iflytop.gd.app.service.device.DeviceStateService;
import com.iflytop.gd.app.service.device.module.GantryModuleService;
import com.iflytop.gd.app.service.device.module.HeatModuleService;
import com.iflytop.gd.common.command.CommandFuture;
import com.iflytop.gd.common.command.DeviceCommandBundle;
import com.iflytop.gd.common.command.DeviceCommandGenerator;
@ -29,6 +32,8 @@ public class TestService {
private final GDDeviceStatusService gdDeviceStatusService;
private final DeviceStateService deviceStateService;
private final DeviceCommandService deviceCommandService;
private final HeatModuleService heatModuleService;
private final GantryModuleService gantryModuleService;
public List<AllSensorDTO> getAllSensor() throws HardwareException {
List<AllSensorDTO> allSensorDTOList = new ArrayList<>();
@ -70,20 +75,38 @@ public class TestService {
long endTime = System.nanoTime();
long duration = endTime - startTime;
double milliseconds = duration / 1_000_000.0;
log.info("指令包装时间：{}",milliseconds);
log.info("指令包装时间：{}", milliseconds);
startTime = System.nanoTime();
CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(deviceCommand);
endTime = System.nanoTime();
duration = endTime - startTime;
milliseconds = duration / 1_000_000.0;
log.info("指令发送时间：{}",milliseconds);
log.info("指令发送时间：{}", milliseconds);
CommandUtil.wait(deviceCommandFuture);
endTime = System.nanoTime();
duration = endTime - startTime;
milliseconds = duration / 1_000_000.0;
log.info("指令反馈时间：{}",milliseconds);
log.info("指令反馈时间：{}", milliseconds);
deviceCommand = DeviceCommandGenerator.fan1Close();
deviceCommandService.sendCommand(deviceCommand);
}
public void moveManyPoints() throws Exception {
Point3D heatArea1TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_01);
Point3D heatArea2TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_02);
Point3D heatArea3TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_03);
Point3D heatArea4TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_04);
Point3D heatArea5TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_05);
Point3D heatArea6TrayClawPoint3D = heatModuleService.getHeatAreaTrayClawPoint3D(HeatModuleCode.heat_module_06);
gantryModuleService.moveManyPoints(
heatArea3TrayClawPoint3D,
heatArea2TrayClawPoint3D,
heatArea1TrayClawPoint3D,
heatArea4TrayClawPoint3D,
heatArea5TrayClawPoint3D,
heatArea6TrayClawPoint3D
);
}
}

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

@ -351,7 +351,7 @@ public class CraftsStepService {
log.info("工艺{},提升拍子存放区至拍子夹取的高度", heatModuleCode);
capModuleService.capUpBalanceNoWait();//提升拍子存放区至拍子夹取的高度
log.info("工艺{},将机械臂移动至加液模块上方", heatModuleCode);
gantryModuleService.gantryMove(liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方
gantryModuleService.gantryXYMove(liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方
log.info("工艺{},将夹爪打开，准备夹取托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，准备夹取托盘
log.info("工艺{},下降z轴,使夹爪落入托盘孔位", heatModuleCode);
@ -366,7 +366,7 @@ public class CraftsStepService {
log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode);
// gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
log.info("工艺{},将携带托盘的机械臂移动至加热模块上方", heatModuleCode);
gantryModuleService.gantryMove(heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
gantryModuleService.gantryXYMove(heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
log.info("工艺{},临时避让完毕，升起", heatModuleCode);
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode);//TODO 结构有问题临时避让 完毕，可以升起了，顺带提升目标加热模块
Thread.sleep(3200);
@ -381,7 +381,7 @@ public class CraftsStepService {
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode);
gantryModuleService.gantryMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(null);//TODO 结构有问题临时避让 开始，移动托盘之前先降下所有加热模块
log.info("工艺{},将夹爪打开，准备夹取拍子", heatModuleCode);
gantryModuleService.clawMove(clawCapPick);//将夹爪打开，准备夹取拍子
@ -392,7 +392,7 @@ public class CraftsStepService {
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},将携带拍子的机械臂移动至加热模块拍子上方", heatModuleCode);
gantryModuleService.gantryMove(heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方
gantryModuleService.gantryXYMove(heatAreaCapClawPointPoint3D);//将携带拍子的机械臂移动至加热模块拍子上方
log.info("工艺{},提升拍子存放区", heatModuleCode);
capModuleService.capUpBalanceNoWait(); //提升拍子存放区
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(heatModuleCode);
@ -405,7 +405,7 @@ public class CraftsStepService {
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode);
gantryModuleService.gantryMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
log.info("工艺{},释放加液区", heatModuleCode);
solutionModuleService.releaseSolutionModule();//释放加液区
}
@ -438,7 +438,7 @@ public class CraftsStepService {
log.info("工艺{},提升拍子存放区至拍子夹取的高度", heatModuleCode);
capModuleService.capUpBalanceNoWait(); //提升拍子存放区至拍子夹取的高度
log.info("工艺{},将机械臂移动至加热模块拍子上方", heatModuleCode);
gantryModuleService.gantryMove(heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
gantryModuleService.gantryXYMove(heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//将加热模块托盘状态改为抬起
log.info("工艺{},抬升指定加热位托盘", heatModuleCode);
heatModuleService.heaterMotorMove(heatModuleCode, trayLift);//抬升指定加热位托盘
@ -455,7 +455,7 @@ public class CraftsStepService {
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);
log.info("工艺{},将携带拍子的机械臂移动至存放区上方", heatModuleCode);
gantryModuleService.gantryMove(capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(null);//TODO 临时避 恢复抬起状态
log.info("工艺{},下降z轴,使夹拍子落入存放区", heatModuleCode);
gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区
@ -465,7 +465,7 @@ public class CraftsStepService {
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},将机械臂移动至加热模块上方", heatModuleCode);
gantryModuleService.gantryMove(heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
gantryModuleService.gantryXYMove(heatAreaTrayClawPoint3D);//将机械臂移动至加热模块上方
log.info("工艺{},将夹爪打开，准备夹取托盘", heatModuleCode);
gantryModuleService.clawMove(clawTrayPick);//将夹爪打开，准备夹取托盘
log.info("工艺{},下降z轴,使夹爪落入托盘孔位", heatModuleCode);
@ -482,7 +482,7 @@ public class CraftsStepService {
log.info("工艺{},将携带托盘的机械臂移动至4号加热模块上方", heatModuleCode);
// gantryModuleService.gantryMove(heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
log.info("工艺{},将携带托盘的机械臂移动至加液模块上方", heatModuleCode);
gantryModuleService.gantryMove(liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
gantryModuleService.gantryXYMove(liquidAreaTrayPoint3D); //将携带托盘的机械臂移动至加液模块上方
log.info("工艺{},临时避 恢复抬起状态", heatModuleCode);
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(null, heatModuleCode);//TODO 临时避 恢复抬起状态
log.info("工艺{},下降z轴,使托盘落入加液模块", heatModuleCode);
@ -494,6 +494,6 @@ public class CraftsStepService {
log.info("工艺{},抬升z轴", heatModuleCode);
gantryModuleService.gantryZMove(0);//抬升z轴
log.info("工艺{},移动机械臂至拍子存放区上方", heatModuleCode);
gantryModuleService.gantryMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
gantryModuleService.gantryXYMove(capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
}
}

2
src/main/java/com/iflytop/gd/app/service/device/DeviceStepCommandService.java
View File

@ -29,7 +29,7 @@ public class DeviceStepCommandService {
private final GDDeviceStatusService gdDeviceStatusService;
public void gantryToPoint3D(Point3D point3D) throws Exception {
gantryModuleService.gantryMove(point3D);
gantryModuleService.gantryXYMove(point3D);
}
/**

45
src/main/java/com/iflytop/gd/app/service/device/module/GantryModuleService.java
View File

@ -16,9 +16,6 @@ import org.springframework.stereotype.Service;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.locks.Condition;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
/**
* 龙门架（转运）模块
@ -31,33 +28,51 @@ public class GantryModuleService {
private final DevicePositionService devicePositionService;
private final DeviceStateService deviceStateService;
/**
* 多点依次移动
*/
public void moveManyPoints(Point3D... point3DS) throws Exception {
moveManyPoints(null, null, point3DS);
}
/**
* 多点依次移动
*/
public void moveManyPoints(String commandId, String command, Point3D... point3DS) throws Exception {
for (Point3D point3D : point3DS) {
gantryXYMove(commandId, command, point3D);
}
}
/**
* 龙门架机械臂移动到0点
*/
public void gantryMoveZero() throws Exception {
gantryMove(null, null, new Point3D(0.0, 0.0, 0.0));
gantryXYMove(null, null, new Point3D(0.0, 0.0, 0.0));
}
/**
* 龙门架机械臂移动到指定点
*/
public void gantryMove(Point3D point) throws Exception {
gantryMove(null, null, point);
public void gantryXYMove(Point3D point) throws Exception {
gantryXYMove(null, null, point);
}
/**
* 龙门架机械臂移动到指定点
*/
public void gantryMove(String commandId, String command, Point3D point) throws Exception {
public void gantryXYMove(String commandId, String command, Point3D point) throws Exception {
DeviceCommandBundle gantryXMoveDeviceCommand = DeviceCommandGenerator.gantryXMove(point.getX());
CommandFuture gantryXMoveDeviceCommandFuture = deviceCommandService.sendCommand(commandId, command, gantryXMoveDeviceCommand);
DeviceCommandBundle gantryYMoveDeviceCommand = DeviceCommandGenerator.gantryYMove(point.getY());
CommandFuture gantryYMoveDeviceCommandFuture = deviceCommandService.sendCommand(commandId, command, gantryYMoveDeviceCommand);
CommandUtil.wait(gantryXMoveDeviceCommandFuture, gantryYMoveDeviceCommandFuture);
DeviceCommandBundle gantryZMoveDeviceCommand = DeviceCommandGenerator.gantryZMove(point.getZ());
CommandFuture gantryZMoveDeviceCommandFuture = deviceCommandService.sendCommand(commandId, command, gantryZMoveDeviceCommand);
CommandUtil.wait(gantryZMoveDeviceCommandFuture);
//
// DeviceCommandBundle gantryZMoveDeviceCommand = DeviceCommandGenerator.gantryZMove(point.getZ());
// CommandFuture gantryZMoveDeviceCommandFuture = deviceCommandService.sendCommand(commandId, command, gantryZMoveDeviceCommand);
// CommandUtil.wait(gantryZMoveDeviceCommandFuture);
}
/**
@ -394,14 +409,14 @@ public class GantryModuleService {
double trayLift = devicePositionService.getPosition(DevicePositionCode.trayLift).getDistance(); //托盘升降抬升距离
List<CommandFuture> futuresList = new ArrayList<>();
for(HeatModuleCode heatModuleCode : HeatModuleCode.values()){
for (HeatModuleCode heatModuleCode : HeatModuleCode.values()) {
DeviceCommandBundle deviceCommand = getHeaterMotorMoveDeviceCommand(heatModuleCode, trayLift);
CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
futuresList.add(deviceCommandFuture);
}
CommandUtil.wait(futuresList.toArray(new CommandFuture[0]));
for(HeatModuleCode heatModuleCode : HeatModuleCode.values()){
for (HeatModuleCode heatModuleCode : HeatModuleCode.values()) {
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(1);//加热模块托盘升降状态
}
}
@ -411,14 +426,14 @@ public class GantryModuleService {
*/
public void allTrayDown(String cmdId, String cmdCode) throws Exception {
List<CommandFuture> futuresList = new ArrayList<>();
for(HeatModuleCode heatModuleCode : HeatModuleCode.values()){
for (HeatModuleCode heatModuleCode : HeatModuleCode.values()) {
DeviceCommandBundle deviceCommand = getHeaterMotorMoveDeviceCommand(heatModuleCode, 0.0);
CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
futuresList.add(deviceCommandFuture);
}
CommandUtil.wait(futuresList.toArray(new CommandFuture[0]));
for(HeatModuleCode heatModuleCode : HeatModuleCode.values()){
for (HeatModuleCode heatModuleCode : HeatModuleCode.values()) {
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(0);//加热模块托盘升降状态
}
}

Write Preview
Loading…
Cancel
Save