diff --git a/src/main/java/a8k/a8k_can_protocol/A8kEcode.java b/src/main/java/a8k/a8k_can_protocol/A8kEcode.java index 2d81daa..19c4816 100644 --- a/src/main/java/a8k/a8k_can_protocol/A8kEcode.java +++ b/src/main/java/a8k/a8k_can_protocol/A8kEcode.java @@ -22,7 +22,7 @@ public enum A8kEcode { MotorLostStep(16, "电机丢步"),// ActionOvertime(17, "动作执行超时"),// CodeException(18, "代码异常"),// - + ShakeModGripperZMNotInZeroPos(19, "抓手升降电机没有在零位"),// // // FOR HARDWARE diff --git a/src/main/java/a8k/appbean/ecode/AppRet.java b/src/main/java/a8k/appbean/ecode/AppRet.java index 38c4ef0..4754a34 100644 --- a/src/main/java/a8k/appbean/ecode/AppRet.java +++ b/src/main/java/a8k/appbean/ecode/AppRet.java @@ -42,7 +42,7 @@ public class AppRet { AppRet r = new AppRet<>(); r.success = true; r.ecode = null; - r.data = data; + r.data = data; return r; } @@ -51,7 +51,7 @@ public class AppRet { AppRet r = new AppRet<>(); r.success = true; r.ecode = null; - r.data = null; + r.data = null; return r; } @@ -59,7 +59,7 @@ public class AppRet { AppRet r = new AppRet<>(); r.success = false; r.ecode = new AppRetEcodeInfo(errorCode, mid, cmd); - r.data = null; + r.data = null; return r; } @@ -67,7 +67,7 @@ public class AppRet { AppRet r = new AppRet<>(); r.success = false; r.ecode = new AppRetEcodeInfo(errorCode, mid, null); - r.data = null; + r.data = null; return r; } @@ -75,7 +75,15 @@ public class AppRet { AppRet r = new AppRet<>(); r.success = false; r.ecode = new AppRetEcodeInfo(errorCode, null, null); - r.data = null; + r.data = null; + return r; + } + + public static AppRet fail(AppRet ret) { + AppRet r = new AppRet<>(); + r.success = false; + r.ecode = new AppRetEcodeInfo(ret.getEcode().errorCode, ret.getEcode().mid, ret.getEcode().cmd); + r.data = null; return r; } @@ -88,13 +96,13 @@ public class AppRet { AppRet r = new AppRet<>(); if (e instanceof HardwareException hexcep) { - r.success = false; - r.ecode = new AppRetEcodeInfo(hexcep.getErrorCode(), hexcep.getModuleId(), hexcep.getCmdId()); + r.success = false; + r.ecode = new AppRetEcodeInfo(hexcep.getErrorCode(), hexcep.getModuleId(), hexcep.getCmdId()); r.data = null; r.traceInfo = trace; } else { - r.success = false; - r.ecode = new AppRetEcodeInfo(A8kEcode.CodeException, null, null); + r.success = false; + r.ecode = new AppRetEcodeInfo(A8kEcode.CodeException, null, null); r.data = null; r.traceInfo = trace; } diff --git a/src/main/java/a8k/base_hardware/A8kCanBusService.java b/src/main/java/a8k/base_hardware/A8kCanBusService.java index 3b10a05..368d2be 100644 --- a/src/main/java/a8k/base_hardware/A8kCanBusService.java +++ b/src/main/java/a8k/base_hardware/A8kCanBusService.java @@ -158,8 +158,31 @@ public class A8kCanBusService { callcmd(id.toInt(), CmdId.code_scaner_stop_scan.toInt()); } - public A8kPacket codeScanerReadScanerResult(MId id) throws HardwareException { - return callcmd(id.toInt(), CmdId.code_scaner_read_scaner_result.toInt()); + public String codeScanerReadScanerResult(MId id) throws HardwareException { + A8kPacket ack = callcmd(id.toInt(), CmdId.code_scaner_read_scaner_result.toInt()); + var contentBytes = ack.getCmdContent(); + return new String(contentBytes); + } + + public Boolean codeScanerResultIsReady(MId id) throws HardwareException { + var packet = callcmd(id.toInt(), CmdId.code_scaner_result_is_ready.toInt()); + return packet.getContentI32(0) != 0; + } + + public String codeScanerWaittingForResult(MId mid, Integer acitionOvertime) throws HardwareException, InterruptedException { + long startedAt = System.currentTimeMillis(); + do { + if (codeScanerResultIsReady(mid)) + break; + long now = System.currentTimeMillis(); + if (now - startedAt > acitionOvertime) { + codeScanerStopScan(mid); + return null; + } + Thread.sleep(100); + } while (true); + codeScanerStopScan(mid); + return codeScanerReadScanerResult(mid); } // @@ -350,7 +373,7 @@ public class A8kCanBusService { callcmd(id.toInt(), CmdId.mini_servo_rotate_with_torque.toInt(), torque); } - public boolean getIOState(IOId ioid) throws HardwareException { + public Boolean getIOState(IOId ioid) throws HardwareException { if (ioid.mtype == ModuleType.kboard) { return callcmd(ioid.mid.toInt(), CmdId.extboard_read_inio.toInt(), ioid.ioIndex).getContentI32(0) != 0; } else if (ioid.mtype == ModuleType.ktmc_step_motor) { @@ -392,6 +415,7 @@ public class A8kCanBusService { } + public void waitForMod(MId mid, Integer acitionOvertime) throws InterruptedException, HardwareException { long startedAt = System.currentTimeMillis(); @@ -412,13 +436,11 @@ public class A8kCanBusService { long now = System.currentTimeMillis(); if (now - startedAt > acitionOvertime) { logger.error("{} waitting for action {} overtime", mid, action); + try {moduleStop(mid);} catch (HardwareException ignored) {} throw new HardwareException(mid, A8kEcode.ActionOvertime); } - - Thread.sleep(100); } while (true); - } public A8kPacket callcmd(Integer moduleId, Integer cmdId) throws HardwareException { diff --git a/src/main/java/a8k/service/db/dao/MotorTubeRackMoveCtrlServiceParameterDao.java b/src/main/java/a8k/service/db/dao/MotorTubeRackMoveCtrlServiceParameterDao.java index 07472ea..faffcbf 100644 --- a/src/main/java/a8k/service/db/dao/MotorTubeRackMoveCtrlServiceParameterDao.java +++ b/src/main/java/a8k/service/db/dao/MotorTubeRackMoveCtrlServiceParameterDao.java @@ -87,4 +87,12 @@ public class MotorTubeRackMoveCtrlServiceParameterDao { public Integer getTubeSpacing() { return HardwareServiceSetting.getInteger(this.serviceName, "TubeSpacing", 0); } + + public Integer getTubeScanServoTorque() { + return HardwareServiceSetting.getInteger(this.serviceName, "TubeScanServoTorque", 0); + } + + public Integer getTubeScanOvertime() { + return HardwareServiceSetting.getInteger(this.serviceName, "TubeScanOvertime", 100); + } } diff --git a/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java b/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java index 7010672..8a5cf03 100644 --- a/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java +++ b/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java @@ -40,7 +40,7 @@ public class MotorTubeRackMoveCtrlService { private CommonHardwareOpeartion comHardwareOpear; /*========================================================================================= - * 私有方法 + * 基础方法 *========================================================================================*/ /** @@ -49,14 +49,34 @@ public class MotorTubeRackMoveCtrlService { * @throws HardwareException 硬件异常 * @throws InterruptedException 打断异常 */ - private void moveTubeRackTo(int pos) throws HardwareException, InterruptedException { - canBus.miniServoMoveTo(MId.ShakeModTubeScanerClampingSV, paramDao.getScanCodeReleasePos()); - canBus.waitForMod(MId.ShakeModTubeScanerClampingSV, paramDao.getActionOvertime()); + public AppRet moveTubeRackTo(int pos) throws HardwareException, InterruptedException { + if (!canBus.stepMotorReadIoState(MId.ShakeModGripperZM)) { + return AppRet.fail(A8kEcode.ShakeModGripperZMNotInZeroPos); + } + scanClampModRelease(); canBus.stepMotorEasyMoveTo(MId.FeedingModXM, pos); canBus.waitForMod(MId.FeedingModXM, paramDao.getActionOvertime()); + return AppRet.success(); + } + + /** + * 扫描夹紧机构夹紧 + */ + public void scanClampModClamp() throws HardwareException, InterruptedException { + canBus.miniServoMoveTo(MId.ShakeModTubeScanerClampingSV, paramDao.getScanCodeClampPos()); + canBus.waitForMod(MId.ShakeModTubeScanerClampingSV, paramDao.getActionOvertime()); + } + + /** + * 扫描夹紧机构复位 + */ + public void scanClampModRelease() throws HardwareException, InterruptedException { + canBus.miniServoMoveTo(MId.ShakeModTubeScanerClampingSV, paramDao.getScanCodeReleasePos()); + canBus.waitForMod(MId.ShakeModTubeScanerClampingSV, paramDao.getActionOvertime()); } + /*========================================================================================= * 公共方法 *========================================================================================*/ @@ -66,7 +86,7 @@ public class MotorTubeRackMoveCtrlService { * @param enable 1:使能 0:禁用 * @throws HardwareException 硬件异常 */ - public AppRet modGroupEnable(Boolean enable) throws HardwareException { + public AppRet modGroupEnable(Boolean enable) throws HardwareException { comHardwareOpear.enableAllMotor(enable); return AppRet.success(); } @@ -79,223 +99,173 @@ public class MotorTubeRackMoveCtrlService { comHardwareOpear.forceStopAllMOtor(); } - public void modGroupMoveToZero() throws HardwareException, InterruptedException { - Boolean state = canBus.stepMotorReadIoState(MId.ShakeModGripperZM); - if (!state) { - // return AppRet; + public AppRet modGroupMoveToZero() throws HardwareException, InterruptedException { + if (!canBus.stepMotorReadIoState(MId.ShakeModGripperZM)) { + return AppRet.fail(A8kEcode.ShakeModGripperZMNotInZeroPos); } - - canBus.miniServoMoveTo(MId.ShakeModTubeScanerClampingSV, paramDao.getScanCodeReleasePos()); canBus.waitForMod(MId.ShakeModTubeScanerClampingSV, paramDao.getActionOvertime()); canBus.stepMotorEasyMoveToZero(MId.FeedingModXM); canBus.waitForMod(MId.FeedingModXM, paramDao.getActionOvertime()); + return AppRet.success(); } - /** * 电机归零，电机运行时归零，特点是通过归零判断设备是否丢步 - * @throws HardwareException - * @throws InterruptedException + * @throws HardwareException 硬件异常 + * @throws InterruptedException 打断异常 */ - public void moveToZero() throws HardwareException, InterruptedException { - //先通过move_to 指令移动到零点 - //再通过 move_to_zero 进行归零(设置超时时间)，超时时间，来源MotorParameterDao - //最后读取 寄存器dval,判断move_to_zero移动了多少距离，如果dval过大，则抛出电机丢步异常。 - //1.记录运行前电机所在位置 - //2.使用move_to_zero移动到零点 - //3.读取电机运行完成后所在的位置 - //4.比较dpos和上面两个位置的差值 相差多少，如果超过2mm，即超过20则认为发生过阻转。 - - var params = this.paramDao; - var beforePos = canBus.stepMotorReadPos(MId.FeedingModXM); + public AppRet modGroupMoveToZeroQuick() throws HardwareException, InterruptedException { + var ret = moveTubeRackTo(0); + if (!ret.isSuccess()) { + return AppRet.fail(ret); + } + moveTubeRackTo(0); canBus.stepMotorEasyMoveToZero(MId.FeedingModXM); canBus.waitForMod(MId.FeedingModXM, paramDao.getActionOvertime()); - var afterPos = canBus.stepMotorReadPos(MId.FeedingModXM); - var distance = beforePos - afterPos; - - var dval = canBus.module_read_reg(MId.FeedingModXM.toInt(), RegIndex.kreg_step_motor_dpos); - if (Math.abs(dval - distance) > params.getHorizontalMotorLostStepThresholdVal()) { - throw new HardwareException(MId.FeedingModXM, A8kEcode.MotorLostStep); - } + return AppRet.success(); } - // - // 试管架移动 - // /** * 移动试管架到入口位置 */ - public void moveTubeRackToEnterPos() throws HardwareException, InterruptedException { - /* - * 1. 先控制试管架夹紧机构复位，让开通道 - * 2. 移动到入口位置 - */ - canBus.miniServoMoveTo(MId.ShakeModTubeScanerClampingSV, paramDao.getScanCodeReleasePos()); - canBus.waitForMod(MId.ShakeModTubeScanerClampingSV, paramDao.getActionOvertime()); - - canBus.stepMotorEasyMoveTo(MId.FeedingModXM, paramDao.getTubeRackEnterPos()); - canBus.waitForMod(MId.FeedingModXM, paramDao.getActionOvertime()); + public AppRet tubeRackMoveToEnterPos() throws HardwareException, InterruptedException { + return moveTubeRackTo(paramDao.getTubeRackEnterPos()); } /** * 移动试管架到出口位置 */ - public void moveTubeRackToExitPos() throws HardwareException, InterruptedException { - /* - * 1. 先控制试管架夹紧机构复位，让开通道 - */ - canBus.miniServoMoveTo(MId.ShakeModTubeScanerClampingSV, paramDao.getScanCodeReleasePos()); - canBus.waitForMod(MId.ShakeModTubeScanerClampingSV, paramDao.getActionOvertime()); - - canBus.stepMotorEasyMoveTo(MId.FeedingModXM, paramDao.getTubeRackExitPos()); - canBus.waitForMod(MId.FeedingModXM, paramDao.getActionOvertime()); + public AppRet moveTubeRackToExitPos() throws HardwareException, InterruptedException { + return moveTubeRackTo(paramDao.getTubeRackExitPos()); } /** * 移动试管架到试管架扫码位置 */ - public void moveTubeRackToScanPos() throws HardwareException, InterruptedException { - canBus.miniServoMoveTo(MId.ShakeModTubeScanerClampingSV, paramDao.getScanCodeReleasePos()); - canBus.waitForMod(MId.ShakeModTubeScanerClampingSV, paramDao.getActionOvertime()); - - canBus.stepMotorEasyMoveTo(MId.FeedingModXM, paramDao.getTubeRackScanCodePos()); - canBus.waitForMod(MId.FeedingModXM, paramDao.getActionOvertime()); + public AppRet moveTubeRackToScanPos() throws HardwareException, InterruptedException { + return moveTubeRackTo(paramDao.getTubeRackScanCodePos()); } /** * 移动试管架到试管架扫码并扫码 */ - public String moveTubeRackToScanPosAndScan() throws HardwareException, InterruptedException { - moveTubeRackToScanPos(); - canBus.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_start_scan.toInt()); - Thread.sleep(5000); - var response = this.canBus.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_read_scaner_result.toInt()); - this.canBus.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_stop_scan.toInt()); - var contentBytes = response.getCmdContent(); - return new String(contentBytes); - // this.moveTubeRackToScanPos(); - // moveTubeRackToScanPos(); - // canBus. - // return ""; + public AppRet moveTubeRackToScanPosAndScan() throws HardwareException, InterruptedException { + String result; + var ret = moveTubeRackToScanPos(); + if (!ret.isSuccess()) { + return AppRet.fail(ret); + } + scanClampModClamp(); + canBus.codeScanerStartScan(MId.FeedingModScannerMod); + result = canBus.codeScanerWaittingForResult(MId.FeedingModScannerMod, 1000); + scanClampModRelease(); + return AppRet.success(result); } // // 试管移动 // - /** * 移动试管到扫码位置 - * @param tubeType - * @param tubeIndex + * @param tubeIndex 试管索引 */ - public void moveTubeToScanPos(String tubeType, Integer tubeIndex) throws HardwareException, InterruptedException { - var params = this.paramDao; - var scanPos = params.getTubeScanPos() + tubeIndex * params.getTubeSpacing(); - this.canBus.callblockcmd(MId.FeedingModXM.toInt(), CmdId.step_motor_easy_move_to.toInt(), scanPos, params.getActionOvertime()); + public AppRet moveTubeToScanPos(Integer tubeIndex) throws HardwareException, InterruptedException { + var scanPos = paramDao.getTubeScanPos() + tubeIndex * paramDao.getTubeSpacing(); + canBus.stepMotorEasyMoveTo(MId.FeedingModXM, scanPos); + canBus.waitForMod(MId.FeedingModXM, paramDao.getActionOvertime()); + return AppRet.success(); } /** * 移动试管到扫码位置，并扫码 - * @param tubeType - * @param tubeIndex + * @param tubeIndex 试管索引 */ - public String moveTubeToScanPosAndScan(String tubeType, Integer tubeIndex) throws HardwareException, InterruptedException { - this.moveTubeToScanPos(tubeType, tubeIndex); - this.canBus.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_start_scan.toInt()); - this.canBus.callcmd(MId.ShakeModTubeScanerRotateSV.toInt(), CmdId.mini_servo_rotate_with_torque.toInt(), 200); - for (int i = 0; i < 10; i++) { - var resIsReady = this.canBus.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_result_is_ready.toInt()); - if (1 == resIsReady.getContentI32(0)) { - break; - } - UfCommon.delay(500); + public AppRet moveTubeToScanPosAndScan(Integer tubeIndex) throws HardwareException, InterruptedException { + var ret = this.moveTubeToScanPos(tubeIndex); + if (!ret.isSuccess()) {return AppRet.fail(ret);} + + try { + scanClampModClamp(); + canBus.codeScanerStartScan(MId.FeedingModScannerMod); + canBus.miniServoRotateWithTorque(MId.ShakeModTubeScanerClampingSV, paramDao.getTubeScanServoTorque()); + String result = canBus.codeScanerWaittingForResult(MId.FeedingModScannerMod, paramDao.getTubeScanOvertime()); + return AppRet.success(result); + } finally { + canBus.moduleStop(MId.ShakeModTubeScanerClampingSV); + canBus.moduleStop(MId.FeedingModScannerMod); + scanClampModRelease(); } - this.canBus.callcmd(MId.ShakeModTubeScanerRotateSV.toInt(), CmdId.mini_servo_stop.toInt(), 1); - var response = this.canBus.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_read_scaner_result.toInt()); - this.canBus.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_stop_scan.toInt()); - var contentBytes = response.getCmdContent(); - return new String(contentBytes); } /** * 移动试管到试管高低判断位置 - * @param tubeType - * @param tubeIndex + * @param tubeIndex 试管索引 */ - public void moveTubeToHeighJudgmentPos(String tubeType, Integer tubeIndex) throws HardwareException, InterruptedException { - var params = this.paramDao; - var detectPos = params.getTubeHeighJudgmentPos() + tubeIndex * params.getTubeSpacing(); - this.canBus.callblockcmd(MId.FeedingModXM.toInt(), CmdId.step_motor_easy_move_to.toInt(), detectPos, params.getActionOvertime()); + public void moveTubeToHeighJudgmentPos(Integer tubeIndex) throws HardwareException, InterruptedException { + moveTubeRackTo(paramDao.getTubeHeighJudgmentPos() + tubeIndex * paramDao.getTubeSpacing()); } /** * 移动试管到试管预处理的位置 - * @param tubeType - * @param tubeIndex + * @param tubeIndex 试管索引 */ - public void moveTubeToPreProcessPos(String tubeType, Integer tubeIndex) throws HardwareException, InterruptedException { - var params = this.paramDao; - var pos = params.getTubePreProcessPos() + tubeIndex * params.getTubeSpacing(); - this.canBus.callblockcmd(MId.FeedingModXM.toInt(), CmdId.step_motor_easy_move_to.toInt(), pos, params.getActionOvertime()); + public void moveTubeToPreProcessPos(Integer tubeIndex) throws HardwareException, InterruptedException { + moveTubeRackTo(paramDao.getTubePreProcessPos() + tubeIndex * paramDao.getTubeSpacing()); } - // - // 状态检测 - // - /** * 移动试管到试管有无判断位置 - * @param tubeType - * @param tubeIndex + * @param tubeIndex 试管索引 */ - public void moveTubeToExistJudgmentPos(String tubeType, Integer tubeIndex) throws HardwareException, InterruptedException { - var params = this.paramDao; - var pos = params.getTubeExistJudgmentPos() + tubeIndex * params.getTubeSpacing(); - this.canBus.callblockcmd(MId.FeedingModXM.toInt(), CmdId.step_motor_easy_move_to.toInt(), new Integer[]{pos}, params.getActionOvertime()); + public void moveTubeToExistJudgmentPos(Integer tubeIndex) throws HardwareException, InterruptedException { + moveTubeRackTo(paramDao.getTubeExistJudgmentPos() + tubeIndex * paramDao.getTubeSpacing()); } - public boolean isTubeRackInEnterPos() throws HardwareException { - return this.canBus.getIOState(IOId.InfeedPPS); + public void tryEnterTubeRack(Integer time) throws HardwareException, InterruptedException { + try { + canBus.stepMotorEasyRotate(MId.FeedingModInfeedM, 1); + Thread.sleep(time); + } finally { + canBus.moduleStop(MId.FeedingModInfeedM); + } } - public boolean isTubeRackInExitPos() throws HardwareException { - return this.canBus.getIOState(IOId.OutfeedPPS); - } - /** - * 获取移动通道的光电状态 - * @param photoelectricIndex 0:里光电，1:外光电 - * @return true, 光电触发 false,光电没有触发 - */ - public boolean getMoveChannelPhotoelectricState(Integer photoelectricIndex) throws HardwareException { - var io = 0 == photoelectricIndex ? IOId.THChOuterPPS : IOId.THChInterPPS; - return this.canBus.getIOState(io); + public void tryEjectTubeRack(Integer time) throws HardwareException, InterruptedException { + try { + canBus.stepMotorEasyRotate(MId.FeedingModOutfeedM, 1); + Thread.sleep(time); + } finally { + canBus.moduleStop(MId.FeedingModOutfeedM); + } } // - // 其他辅助组件 + // 状态 // - /** - * 扫描夹紧机构夹紧 - */ - public void scanClampModuleCompress() throws HardwareException, InterruptedException { - var params = this.paramDao; - this.canBus.callblockcmd(MId.ShakeModTubeScanerClampingSV.toInt(), CmdId.mini_servo_move_to.toInt(), new Integer[]{params.getScanCodeClampPos()}, params.getActionOvertime()); + public AppRet isTubeRackInEnterPos() throws HardwareException { + return AppRet.success(canBus.getIOState(IOId.InfeedPPS)); } - /** - * 扫描夹紧机构复位 - */ - public void scanClampModuleReset() throws HardwareException, InterruptedException { - var params = this.paramDao; - this.canBus.callblockcmd(MId.ShakeModTubeScanerClampingSV.toInt(), CmdId.mini_servo_move_to.toInt(), new Integer[]{params.getScanCodeReleasePos()}, params.getActionOvertime()); + public AppRet isTubeRackInExitPos() throws HardwareException { + return AppRet.success(canBus.getIOState(IOId.OutfeedPPS)); } + public AppRet isHighTube() throws HardwareException { + return AppRet.success(canBus.getIOState(IOId.TubeHeightPPS)); + } + + public AppRet getTHchOuterPPS() throws HardwareException { + return AppRet.success(canBus.getIOState(IOId.THChOuterPPS)); + } + public AppRet getTHchInterPPS() throws HardwareException { + return AppRet.success(canBus.getIOState(IOId.THChInterPPS)); + } }