Browse Source

修复拍子升降错误

master
白凤吉 2 months ago
parent
commit
cbd0aeb1db
  1. 21
      src/main/java/com/iflytop/gd/app/command/control/MoveToHeatAreaCommand.java
  2. 21
      src/main/java/com/iflytop/gd/app/command/control/MoveToSolutionAreaCommand.java
  3. 67
      src/main/java/com/iflytop/gd/app/controller/TestController.java
  4. 12
      src/main/java/com/iflytop/gd/app/service/api/TestService.java
  5. 86
      src/main/java/com/iflytop/gd/app/service/device/module/CapModuleService.java
  6. 1
      src/main/java/com/iflytop/gd/common/result/ResultCode.java

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

@ -69,10 +69,10 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
//校验目标加热位是否有托盘
try {
deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(true);
// Boolean heatModuleTray = heatModuleService.heatModuleSensorState(heatModuleCode);
// if (heatModuleTray) {
Boolean heatModuleTray = heatModuleService.heatModuleSensorState(heatModuleCode);
if (heatModuleTray) {
// throw new AppException(ResultCode.TARGET_HEAT_MODULE_OCCUPIED);
// }
}
} catch (Exception e) {
deviceStateService.getCommandMutexState().get().setMoveToHeatAreaCommandExecuting(false);
throw e;
@ -80,37 +80,38 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler {
return runAsync(() -> {
try {
// TrayState trayState = deviceStateService.getTrayInSolutionModule();
capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand());//提升拍子存放区至拍子夹取的高度
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), liquidAreaTrayPoint3D); //将机械臂移动至加液模块上方
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开准备夹取托盘
gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使夹爪落入托盘孔位
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧夹住托盘
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(0);//加液模块是否有托盘
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidDown(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDown(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 结构有问题临时避让 开始移动托盘之前先降下所有加热模块
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatArea4TrayClawPoint3D);//将携带托盘的机械臂移动至4号加热模块上方
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D);//将携带托盘的机械臂移动至加热模块上方
// deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLift);//抬升加热位托盘
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);Thread.sleep(3200);//TODO 结构有问题临时避让 完毕可以升起了顺带提升目标加热模块
// deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLift);//抬升加热位托盘 TODO 结构有问题临时避让 屏蔽
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//TODO 结构有问题临时避让 完毕可以升起了顺带提升目标加热模块
gantryModuleService.gantryZMove(heatModuleTrayMoveHeight);//下降z轴,使托盘落入加热模块
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayPick);//将夹爪打开释放托盘
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(1);//加热模块是否存在托盘
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
capModuleService.capUpBalance(cmdDTO.getCommandId(), cmdDTO.getCommand());//提升拍子存放区至拍子夹取的高度
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
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()); //提升拍子存放区
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轴
// trayState.setHeatModuleId(heatModuleId);
// trayState.setInHeatModule(true);
// heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLower);//下降加热模块托盘
// deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(0);//加热模块托盘升降状态
heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLower);//下降加热模块托盘
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayUp(0);//加热模块托盘升降状态
capModuleService.capUpBalance(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//移动机械臂至拍子存放区上方
} finally {
deviceStateService.getDeviceState().getGantryArm().setIdle(true);

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

@ -47,9 +47,6 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
|| deviceStateService.getCommandMutexState().get().isShakeStartCommandExecuting()) {
throw new AppException(ResultCode.CMD_BUSY);
}
if (!deviceStateService.getDeviceState().getSolutionModule().isIdle()) {
throw new AppException(ResultCode.SOLUTION_MODULE_OCCUPIED);
}
String heatId = cmdDTO.getStringParam("heatId");
HeatModuleCode heatModuleCode = HeatModuleCode.valueOf(heatId);
double clawCapPick = devicePositionService.getPosition(DevicePositionCode.clawCapPick).getDistance(); //获取拍子夹取距离
@ -72,10 +69,10 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
//校验目标加热位是否有托盘
try {
deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(true);
// Boolean heatModuleTray = heatModuleService.heatModuleSensorState(heatModuleCode);
// if (!heatModuleTray) {
Boolean heatModuleTray = heatModuleService.heatModuleSensorState(heatModuleCode);
if (!heatModuleTray) {
// throw new AppException(ResultCode.TARGET_HEAT_MODULE_NO_TRAY);
// }
}
} catch (Exception e) {
deviceStateService.getCommandMutexState().get().setMoveToSolutionAreaCommandExecuting(false);
throw e;
@ -84,15 +81,15 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
try {
// TrayState trayState = deviceStateService.getTrayInSolutionModule();
solutionModuleService.requestSolutionModule(cmdDTO.getCommandId(), cmdDTO.getCommand());//申请使用加液区并等待
capModuleService.capUpBalanceNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区至拍子夹取的高度
capModuleService.capUpBalance(cmdDTO.getCommandId(), cmdDTO.getCommand()); //提升拍子存放区至拍子夹取的高度
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaCapClawPointPoint3D);//将机械臂移动至加热模块拍子上方
heatModuleService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode, trayLift);//抬升指定加热位托盘
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开准备夹取拍子
gantryModuleService.gantryZMove(heatModuleCapMoveHeight);//下降z轴,使夹爪落入加热模块拍子孔位
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapGrip);//将夹爪收紧夹住拍子
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setCapExist(false);//加热模块是否存在拍子
capModuleService.capMotorMoveByNumNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), -1);//拍子存放模块下降1个拍子位置
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0); Thread.sleep(2000);//抬升z轴
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
capModuleService.capMotorMoveByNum(-1);//拍子存放模块下降1个拍子位置
gantryModuleService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), capStorageCapClawPoint3D);//将携带拍子的机械臂移动至存放区上方
gantryModuleService.gantryZMove(capModuleCapMoveHeight);//下降z轴,使夹拍子落入存放区
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawCapPick);//将夹爪打开释放夹取的拍子
@ -105,13 +102,13 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler {
gantryModuleService.clawMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), clawTrayGrip);//将夹爪收紧夹住托盘
deviceStateService.getDeviceState().getHeatModuleByCode(heatModuleCode).setTrayStatus(0);//加热模块是否存在托盘
// trayState.setInHeatModule(false);//托盘是否在加热模块中
deviceCommandTempUtilService.moveTrayHeatModuleAvoidDownNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleCode);//TODO 临时避让下降
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);Thread.sleep(2500);//抬升z轴
gantryModuleService.gantryZMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), 0);//抬升z轴
// 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); //将携带托盘的机械臂移动至加液模块上方
// deviceCommandTempUtilService.moveTrayHeatModuleAvoidUpNoWait(cmdDTO.getCommandId(), cmdDTO.getCommand(), null, heatModuleCode);//TODO 临时避 恢复抬起状态
deviceCommandTempUtilService.moveTrayHeatModuleAvoidUp(cmdDTO.getCommandId(), cmdDTO.getCommand(), null);//TODO 临时避 恢复抬起状态
gantryModuleService.gantryZMove(solutionModuleTrayMoveHeight);//下降z轴,使托盘落入加液模块
// trayState.setInSolutionModule(true);//托盘是否在加液模块中
deviceStateService.getDeviceState().getSolutionModule().setTrayStatus(1);//加液模块是否存在托盘

67
src/main/java/com/iflytop/gd/app/controller/TestController.java

@ -1,20 +1,15 @@
package com.iflytop.gd.app.controller;
import com.iflytop.gd.app.model.bo.status.CommandMutexState;
import com.iflytop.gd.app.model.dto.AllSensorDTO;
import com.iflytop.gd.app.service.api.HeatService;
import com.iflytop.gd.app.service.api.TestService;
import com.iflytop.gd.app.service.device.DeviceEmergencyStopService;
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.OtherModuleService;
import com.iflytop.gd.common.enums.HeatModuleCode;
import com.iflytop.gd.common.result.Result;
import com.iflytop.gd.hardware.exception.HardwareException;
import io.swagger.v3.oas.annotations.Operation;
import io.swagger.v3.oas.annotations.tags.Tag;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.web.bind.annotation.GetMapping;
import org.springframework.web.bind.annotation.PostMapping;
import org.springframework.web.bind.annotation.RequestMapping;
import org.springframework.web.bind.annotation.RestController;
@ -32,27 +27,9 @@ import java.util.List;
public class TestController {
private final TestService testService;
private final CapModuleService capModuleService;
private final DeviceStateService deviceStateService;
private final OtherModuleService otherModuleService;
private final DeviceEmergencyStopService deviceEmergencyStopService;
@Operation(summary = "启动虚拟模式")
@PostMapping("/virtual")
public Result<?> changeVirtualMode() {
deviceStateService.getDeviceState().setVirtual(true);
deviceStateService.getDeviceState().setInitComplete(true);
return Result.success();
}
@Operation(summary = "模拟向硬件写入完毕")
@PostMapping("/initComplete")
public Result<?> setInitComplete(Boolean initComplete) {
deviceStateService.getDeviceState().setInitComplete(initComplete);
return Result.success();
}
@Operation(summary = "获取所有传感器读数")
@GetMapping("/all-sensor")
@PostMapping("/all-sensor")
public Result<List<AllSensorDTO>> getAllSensor() throws HardwareException {
return Result.success(testService.getAllSensor());
}
@ -72,44 +49,4 @@ public class TestController {
return Result.success();
}
@Operation(summary = "获取指令互斥状态")
@GetMapping("/mutex-state")
public Result<CommandMutexState> getCommandMutexState() throws HardwareException {
return Result.success(deviceStateService.getCommandMutexState().get());
}
@Operation(summary = "重置状态")
@PostMapping("/reset")
public Result<?> reset() throws Exception {
testService.reset();
return Result.success();
}
@Operation(summary = "触发急停")
@PostMapping("/press-emergency-stop")
public Result<?> pressEmergencyStop() throws Exception {
deviceEmergencyStopService.press();
return Result.success();
}
@Operation(summary = "解除急停")
@PostMapping("/release-emergency-stop")
public Result<?> releaseEmergencyStop() throws Exception {
deviceEmergencyStopService.release();
return Result.success();
}
@Operation(summary = "工艺执行完毕蜂鸣器提醒")
@PostMapping("/crafts-finish-beep-remind")
public Result<?> craftsFinishBeepRemind() throws Exception {
otherModuleService.craftsFinishBeepRemind();
return Result.success();
}
@Operation(summary = "设置当前温度")
@PostMapping("/set-temperature")
public Result<?> setTemperature(HeatModuleCode code, Double temperature) throws Exception {
testService.setTemperature(code, temperature);
return Result.success();
}
}

12
src/main/java/com/iflytop/gd/app/service/api/TestService.java

@ -1,8 +1,6 @@
package com.iflytop.gd.app.service.api;
import com.iflytop.gd.app.model.dto.AllSensorDTO;
import com.iflytop.gd.app.service.device.DeviceStateService;
import com.iflytop.gd.common.enums.HeatModuleCode;
import com.iflytop.gd.hardware.exception.HardwareException;
import com.iflytop.gd.hardware.service.GDDeviceStatusService;
import com.iflytop.gd.hardware.type.IO.InputIOMId;
@ -19,7 +17,6 @@ import java.util.List;
@RequiredArgsConstructor
public class TestService {
private final GDDeviceStatusService gdDeviceStatusService;
private final DeviceStateService deviceStateService;
public List<AllSensorDTO> getAllSensor() throws HardwareException {
List<AllSensorDTO> allSensorDTOList = new ArrayList<>();
@ -41,13 +38,4 @@ public class TestService {
return allSensorDTOList;
}
public void reset(){
deviceStateService.getCommandMutexState().get().reset();
deviceStateService.getDeviceState().reset();
}
public void setTemperature(HeatModuleCode code, Double temperature){
deviceStateService.getDeviceState().getHeatModuleByCode(code).setTemperature(temperature);
}
}

86
src/main/java/com/iflytop/gd/app/service/device/module/CapModuleService.java

@ -30,72 +30,11 @@ public class CapModuleService {
/**
* 将拍子升降区抬升至可以获取的位置
*/
public void capUpBalanceNoWait() throws Exception {
capUpBalanceNoWait(null, null);
}
/**
* 将拍子升降区抬升至可以获取的位置
*/
public void capUpBalance() throws Exception {
capUpBalance(null, null);
}
/**
* 将拍子升降区抬升至可以获取的位置 不等待
*/
public void capUpBalanceNoWait(String commandId, String command) throws Exception {
Boolean capExist01 = gdDeviceStatusService.getInputState(InputIOMId.CAP01_EXIST);
Boolean capExist02 = gdDeviceStatusService.getInputState(InputIOMId.CAP02_EXIST);
Boolean capExist03 = gdDeviceStatusService.getInputState(InputIOMId.CAP03_EXIST);
Boolean capExist04 = gdDeviceStatusService.getInputState(InputIOMId.CAP04_EXIST);
Boolean capExist05 = gdDeviceStatusService.getInputState(InputIOMId.CAP05_EXIST);
Boolean capExist06 = gdDeviceStatusService.getInputState(InputIOMId.CAP06_EXIST);
//如果传感器读数存在null则视为异常
if (capExist01 == null || capExist02 == null || capExist03 == null || capExist04 == null || capExist05 == null || capExist06 == null) {
log.error("拍子存放区获取传感器状态异常");
throw new AppException(ResultCode.SENSOR_STATUS_FAILED);
}
//先统计拍子个数
int capNum = 0;
if (capExist01) {
return;
}
if (capExist02) {
capNum++;
}
if (capExist03) {
capNum++;
}
if (capExist04) {
capNum++;
if (capExist02 && !capExist03) {
throw new AppException(ResultCode.SENSOR_STATUS_FAILED);
}
}
if (capExist05) {
capNum++;
if (capExist03 && !capExist04) {
throw new AppException(ResultCode.SENSOR_STATUS_FAILED);
}
}
if (capExist06) {
capNum++;
if (capExist04 && !capExist05) {
throw new AppException(ResultCode.SENSOR_STATUS_FAILED);
}
}
if (capNum == 0) {
return;
}
double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance();//每个拍子高度
double capLiftingHeightSum = (6 - capNum) * capLiftingHeight + 21;
//先留一个空位置不要完全抬升防止有传感器坏了的情况使用第二个传感器做校验
DeviceCommandBundle deviceCommand = DeviceCommandGenerator.capMotorMove(capLiftingHeightSum);
deviceCommandService.sendCommand(commandId, command, deviceCommand);
}
/**
* 将拍子升降区抬升至可以获取的位置
*/
public void capUpBalance(String commandId, String command) throws Exception {
@ -176,15 +115,6 @@ public class CapModuleService {
*
* @param num 1为上升1格-1为下降1格
*/
public void capMotorMoveByNumNoWait(int num) throws Exception {
capMotorMoveByNumNoWait(null, null, num);
}
/**
* 调整拍子升降电机位置
*
* @param num 1为上升1格-1为下降1格
*/
public void capMotorMoveByNum(int num) throws Exception {
capMotorMoveByNum(null, null, num);
}
@ -194,22 +124,6 @@ public class CapModuleService {
*
* @param num 1为上升1格-1为下降1格
*/
public void capMotorMoveByNumNoWait(String commandId, String command, int num) throws Exception {
double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance();
Double capPosition = gdDeviceStatusService.getStepMotorPostion(StepMotorMId.TRAY_MOTOR_MID);
double toCapPosition = capPosition + (num * capLiftingHeight);
if (toCapPosition < 0) {
throw new AppException(ResultCode.CAP_LIFT_ERROR);
}
DeviceCommandBundle deviceCommand = DeviceCommandGenerator.capMotorMove(toCapPosition);
deviceCommandService.sendCommand(commandId, command, deviceCommand);
}
/**
* 调整拍子升降电机位置
*
* @param num 1为上升1格-1为下降1格
*/
public void capMotorMoveByNum(String commandId, String command, int num) throws Exception {
double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance();
Double capPosition = gdDeviceStatusService.getStepMotorPostion(StepMotorMId.TRAY_MOTOR_MID);

1
src/main/java/com/iflytop/gd/common/result/ResultCode.java

@ -57,6 +57,7 @@ public enum ResultCode implements IResultCode, Serializable {
CAP_LIFT_ERROR("6024", "拍子升降错误"),
CMD_BUSY("6025", "设备忙,请稍后"),
HEAT_MODULE_NO_IDLE("6026", "加热模块无空闲"),
CAP_MODULE_NO_CAP("6027", "拍子存放区未检测到拍子"),
;
/** 状态码 */
private final String code;

Loading…
Cancel
Save