From ce342942472f18ae205e9df05e013fb1f6bf6b0a Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 25 Jul 2024 14:37:06 +0800 Subject: [PATCH] update --- .../a8k/appbean/TargetPosMeasureDirection.java | 5 ++ src/main/java/a8k/appbean/ecode/AppRet.java | 12 +++- .../java/a8k/appbean/ecode/AppRetEcodeInfo.java | 12 ++++ src/main/java/a8k/canbus/A8kCanBusService.java | 46 +++---------- .../hardware/MotorTubeRackMoveCtrlService.java | 78 +++++++++++++--------- 5 files changed, 83 insertions(+), 70 deletions(-) create mode 100644 src/main/java/a8k/appbean/TargetPosMeasureDirection.java diff --git a/src/main/java/a8k/appbean/TargetPosMeasureDirection.java b/src/main/java/a8k/appbean/TargetPosMeasureDirection.java new file mode 100644 index 0000000..4a65124 --- /dev/null +++ b/src/main/java/a8k/appbean/TargetPosMeasureDirection.java @@ -0,0 +1,5 @@ +package a8k.appbean; + +public enum TargetPosMeasureDirection { + NOTCARE, POSITIVE, NEGATIVE; +} diff --git a/src/main/java/a8k/appbean/ecode/AppRet.java b/src/main/java/a8k/appbean/ecode/AppRet.java index a7fa13b..2686b45 100644 --- a/src/main/java/a8k/appbean/ecode/AppRet.java +++ b/src/main/java/a8k/appbean/ecode/AppRet.java @@ -38,6 +38,9 @@ public class AppRet { return traceInfo; } + public String getMessage() { + return message; + } public static AppRet success(T data) { AppRet r = new AppRet<>(); @@ -56,11 +59,14 @@ public class AppRet { return r; } + public static AppRet fail(A8kEcode errorCode, MId mid, CmdId cmd) { AppRet r = new AppRet<>(); r.success = false; r.ecode = new AppRetEcodeInfo(errorCode, mid, cmd); r.data = null; + + r.message = r.ecode.toDisPlayString(); return r; } @@ -69,6 +75,7 @@ public class AppRet { r.success = false; r.ecode = new AppRetEcodeInfo(errorCode, mid, null); r.data = null; + r.message = r.ecode.toDisPlayString(); return r; } @@ -77,6 +84,7 @@ public class AppRet { r.success = false; r.ecode = new AppRetEcodeInfo(errorCode, null, null); r.data = null; + r.message = r.ecode.toDisPlayString(); return r; } @@ -85,6 +93,8 @@ public class AppRet { r.success = false; r.ecode = new AppRetEcodeInfo(ret.getEcode().errorCode, ret.getEcode().mid, ret.getEcode().cmd); r.data = null; + r.message = r.ecode.toDisPlayString(); + return r; } @@ -101,7 +111,7 @@ public class AppRet { r.ecode = new AppRetEcodeInfo(hexcep.getErrorCode(), hexcep.getModuleId(), hexcep.getCmdId()); r.data = null; r.traceInfo = trace; - r.message = e.getMessage(); + r.message = r.ecode.toDisPlayString(); } else { r.success = false; r.ecode = new AppRetEcodeInfo(A8kEcode.CodeException, null, null); diff --git a/src/main/java/a8k/appbean/ecode/AppRetEcodeInfo.java b/src/main/java/a8k/appbean/ecode/AppRetEcodeInfo.java index 7c42975..9b8a696 100644 --- a/src/main/java/a8k/appbean/ecode/AppRetEcodeInfo.java +++ b/src/main/java/a8k/appbean/ecode/AppRetEcodeInfo.java @@ -74,4 +74,16 @@ public class AppRetEcodeInfo { } return cmd.toString(); } + + public String toDisPlayString() { + if (errorCode != null && mid != null && cmd != null) { + return String.format("ECode:%s, MID:%s, CMD:%s", errorCode, mid, cmd); + } else if (errorCode != null && mid != null) { + return String.format("ECode:%s, MID:%s", errorCode, mid); + } else if (errorCode != null) { + return String.format("ECode:%s", errorCode); + } else { + return String.format("ECode:%s, MID:%s, CMD:%s", null, mid, cmd); + } + } } diff --git a/src/main/java/a8k/canbus/A8kCanBusService.java b/src/main/java/a8k/canbus/A8kCanBusService.java index 8530b39..e621a86 100644 --- a/src/main/java/a8k/canbus/A8kCanBusService.java +++ b/src/main/java/a8k/canbus/A8kCanBusService.java @@ -297,7 +297,7 @@ public class A8kCanBusService { callcmd(mid.toInt(), CmdId.step_motor_enable.toInt(), enable); } - public int stepMotorReadPos(MId id) throws HardwareException { + public Integer stepMotorReadPos(MId id) throws HardwareException { A8kPacket packet = callcmd(id.toInt(), CmdId.step_motor_read_pos.toInt()); return packet.getContentI32(0); } @@ -488,17 +488,15 @@ public class A8kCanBusService { // CmdId action = lastCmdMap.get(mid); CmdId action = lastCmdMapGet(mid); do { - - try { - var status = getModuleStatus(mid); - if (status == ModuleStatus.IDLE) { - break; - } else if (status == ModuleStatus.ERROR) { - logger.error("{} waitting for action {} , catch error {}", mid, action, moduleGetError(mid)); - throw new HardwareException(mid, moduleGetError(mid)); - } - } catch (HardwareException ignored) { + ModuleStatus statu = null; + try {statu = getModuleStatus(mid);} catch (HardwareException ignored) {} + if (statu == ModuleStatus.IDLE) { + break; + } else if (statu == ModuleStatus.ERROR) { + logger.error("{} waitting for action {} , catch error {}", mid, action, moduleGetError(mid)); + throw new HardwareException(mid, moduleGetError(mid)); } + long now = System.currentTimeMillis(); if (now - startedAt > acitionOvertime) { logger.error("{} waitting for action {} overtime", mid, action); @@ -535,7 +533,7 @@ public class A8kCanBusService { return sendCmdAutoResend(packet, A8kPacket.CMD_OVERTIME); } - public A8kPacket sendCmdAutoResend(A8kPacket pack, int overtime) throws HardwareException { + synchronized public A8kPacket sendCmdAutoResend(A8kPacket pack, int overtime) throws HardwareException { // 调用sendCmd, // 如果捕获到超时异常，则重发，最多重发三次 // 如果是其他异常，则直接抛出异常。 @@ -555,30 +553,6 @@ public class A8kCanBusService { throw new HardwareException(MId.valueOf(pack.getModuleId()), A8kEcode.Overtime); } - public void callblockcmd(Integer moduleId, Integer cmdId, Integer[] params, int acitionOvertime) throws HardwareException, InterruptedException { - var packet = this.packParamsToPacket(moduleId, cmdId, params); - this.sendCmdAutoResend(packet, A8kPacket.CMD_OVERTIME); - - // 调用sendCmdAutoResend - // 通过kmodule_get_status判断指令是否完成,超时未完成则报错,则调用module_stop,同时抛出异常 - // - - long startedAt = System.currentTimeMillis(); - do { - var pack = this.callcmd(moduleId, CmdId.module_get_status.toInt(), new Integer[]{}); - var status = pack.getContentI32(0); - if (0 == status) { - break; - } - - long now = System.currentTimeMillis(); - if (now - startedAt > acitionOvertime) { - throw new HardwareException(MId.valueOf(pack.getModuleId()), A8kEcode.Overtime); - } - - Thread.sleep(100); - } while (true); - } private A8kPacket sendCmd(A8kPacket pack, int overtime) throws HardwareException { pack.setPacketIndex(packetIndex); diff --git a/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java b/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java index 88c2ef6..67391d9 100644 --- a/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java +++ b/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java @@ -1,5 +1,6 @@ package a8k.service.hardware; +import a8k.appbean.TargetPosMeasureDirection; import a8k.canbus.protocol.*; import a8k.appbean.HardwareException; import a8k.appbean.ecode.AppRet; @@ -52,7 +53,7 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { @HardwareServiceParam(name = "出料X位置", group = "试管架位置信息") public Integer getTRExitXPos() { - return 4001; + return 3980; } @HardwareServiceParam(name = "试管架扫码X位置", group = "试管架位置信息") @@ -127,12 +128,10 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { @Override @HardwareServiceAction(name = "模块归零", group = "基础操作") public void modGroupMoveToZero() throws HardwareException, InterruptedException { - // if (!canBus.stepMotorReadIoState(MId.ShakeModGripperZM, 0)) { - // throw new HardwareException(A8kEcode.ShakeModGripperZMNotInZeroPos); - // } - canBus.miniServoEnable(MId.ShakeModTubeScanerClampingSV, 1); - canBus.miniServoMoveTo(MId.ShakeModTubeScanerClampingSV, 0); - canBus.waitForMod(MId.ShakeModTubeScanerClampingSV, getActionOvertime()); + if (!canBus.stepMotorReadIoState(MId.ShakeModGripperZM, 0)) { + throw new HardwareException(A8kEcode.ShakeModGripperZMNotInZeroPos); + } + scanClampModRelease(); canBus.stepMotorEnable(MId.FeedingModXM, 1); canBus.stepMotorEasyMoveToZero(MId.FeedingModXM); @@ -142,10 +141,7 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { @Override @HardwareServiceAction(name = "模块快速归零", group = "基础操作") public void modGroupMoveToZeroQuick() throws HardwareException, InterruptedException { - moveTubeRackTo(0); - canBus.stepMotorEasyMoveToZero(MId.FeedingModXM); - canBus.waitForMod(MId.FeedingModXM, getActionOvertime()); - + canBus.stepMotorEasyMoveToZeroPointQuickBlock(MId.FeedingModXM, getActionOvertime()); } /*========================================================================================= @@ -159,14 +155,32 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { * @throws InterruptedException 打断异常 */ @HardwareServiceAction(name = "移动到X", group = "模块基础动作") - public void moveTubeRackTo(Integer pos) throws HardwareException, InterruptedException { + public void moveTubeRackTo(Integer pos, TargetPosMeasureDirection moveDiretion, Boolean moveToZero) throws HardwareException, InterruptedException { if (!canBus.stepMotorReadIoState(MId.ShakeModGripperZM, 0)) { throw new HardwareException(A8kEcode.ShakeModGripperZMNotInZeroPos); } + //打开扫码夹具 scanClampModRelease(); + + //使能电机 canBus.stepMotorEnable(MId.FeedingModXM, 1); - canBus.stepMotorEasyMoveTo(MId.FeedingModXM, pos); - canBus.waitForMod(MId.FeedingModXM, getActionOvertime()); + if (moveToZero) { + canBus.stepMotorEasyMoveToZeroPointQuickBlock(MId.FeedingModXM, getActionOvertime()); + } + + // 处理试管架和试管架之间的间隙导致的运行位置误差 + Integer nowPos = canBus.stepMotorReadPos(MId.FeedingModXM); + if (TargetPosMeasureDirection.POSITIVE.equals(moveDiretion)) { + if (nowPos > (pos + 1)) { + canBus.stepMotorEasyMoveToBlock(MId.FeedingModXM, pos + 30, getActionOvertime()); + } + } else if (TargetPosMeasureDirection.NEGATIVE.equals(moveDiretion)) { + if (nowPos < (pos - 1)) { + canBus.stepMotorEasyMoveToBlock(MId.FeedingModXM, pos - 30, getActionOvertime()); + } + } + canBus.stepMotorEasyMoveToBlock(MId.FeedingModXM, pos, getActionOvertime()); + } /** @@ -217,13 +231,12 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { @HardwareServiceAction(name = "通过归零读取当前电机位置", group = "模块基础动作") public AppRet readXPosByMoveZero() throws HardwareException, InterruptedException { - - canBus.stepMotorEnable(MId.FeedingModXM, 1); - canBus.stepMotorEasyMoveToZero(MId.FeedingModXM); - canBus.waitForMod(MId.FeedingModXM, getActionOvertime()); + canBus.stepMotorEasyMoveToZeroBlock(MId.FeedingModXM, getActionOvertime()); + Integer nowpos = canBus.stepMotorReadPos(MId.FeedingModXM); + Integer measurepos = -canBus.moduleGetReg(MId.FeedingModXM, RegIndex.kreg_step_motor_dpos) + nowpos; canBus.stepMotorEnable(MId.FeedingModXM, 0); - return AppRet.success(-canBus.moduleGetReg(MId.FeedingModXM, RegIndex.kreg_step_motor_dpos)); + return AppRet.success(measurepos); } @@ -238,8 +251,7 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { */ @HardwareServiceAction(name = "移动<试管架>到入口位置", group = "片段") public void tubeRackMoveToEnterPos() throws HardwareException, InterruptedException { - - moveTubeRackTo(getTREnterXPos()); + moveTubeRackTo(getTREnterXPos(), TargetPosMeasureDirection.NOTCARE, true); } /** @@ -247,14 +259,16 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { */ @HardwareServiceAction(name = "移动<试管架>到出口位置", group = "片段") public void moveTubeRackToExitPos() throws HardwareException, InterruptedException { - moveTubeRackTo(getTRExitXPos()); + moveTubeRackTo(getTRExitXPos(), TargetPosMeasureDirection.NOTCARE, false); + moveTubeRackTo(getTRExitXPos() - 10, TargetPosMeasureDirection.NOTCARE, false); +// moveTubeRackTo(getTRExitXPos(), TargetPosMeasureDirection.NOTCARE, false); } /** * 移动<试管架>到试管架扫码位置 */ private void moveTubeRackToScanPos() throws HardwareException, InterruptedException { - moveTubeRackTo(getTRScanXPos()); + moveTubeRackTo(getTRScanXPos(), TargetPosMeasureDirection.POSITIVE, false); } /** @@ -279,11 +293,10 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { * 移动试管到扫码位置 * @param tubeIndex 试管索引 */ - private AppRet moveTubeToScanPos(Integer tubeIndex) throws HardwareException, InterruptedException { + private void moveTubeToScanPos(Integer tubeIndex) throws HardwareException, InterruptedException { var scanPos = getTScanXPos() + tubeIndex * getTubeSpacing(); - canBus.stepMotorEasyMoveTo(MId.FeedingModXM, scanPos); - canBus.waitForMod(MId.FeedingModXM, getActionOvertime()); - return AppRet.success(); + scanClampModClamp(); + moveTubeRackTo(scanPos, TargetPosMeasureDirection.NEGATIVE, false); } /** @@ -292,11 +305,10 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { */ @HardwareServiceAction(name = "扫描<试管X>编码", group = "片段") public AppRet moveTubeToScanPosAndScan(Integer tubeIndex) throws HardwareException, InterruptedException { - var ret = this.moveTubeToScanPos(tubeIndex); - if (!ret.isSuccess()) {return AppRet.fail(ret);} - + moveTubeToScanPos(tubeIndex); try { scanClampModClamp(); + canBus.codeScanerStartScan(MId.FeedingModScannerMod); canBus.miniServoRotateWithTorque(MId.ShakeModTubeScanerClampingSV, getTubeScanServoTorque()); String result = canBus.codeScanerWaittingForResult(MId.FeedingModScannerMod, getTubeScanOvertime()); @@ -314,7 +326,7 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { */ @HardwareServiceAction(name = "判断<试管N>高低", group = "片段") public AppRet moveTubeToAltitJudgXPos(Integer tubeIndex) throws HardwareException, InterruptedException { - moveTubeRackTo(getTAltitJudgXPos() + tubeIndex * getTubeSpacing()); + moveTubeRackTo(getTAltitJudgXPos() + tubeIndex * getTubeSpacing(), TargetPosMeasureDirection.NEGATIVE, false); return AppRet.success(isHighTube()); } @@ -324,7 +336,7 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { */ @HardwareServiceAction(name = "移动<试管N>摇匀位", group = "片段") public void moveTubeToPreProcessPos(Integer tubeIndex) throws HardwareException, InterruptedException { - moveTubeRackTo(getTPreProcessXPos() + tubeIndex * getTubeSpacing()); + moveTubeRackTo(getTPreProcessXPos() + tubeIndex * getTubeSpacing(), TargetPosMeasureDirection.POSITIVE, false); } /** @@ -333,7 +345,7 @@ public class MotorTubeRackMoveCtrlService implements HardwareCtrlModule { */ @HardwareServiceAction(name = "判断<试管N>是否存在", group = "片段") public AppRet judgeTubeExist(Integer tubeIndex) throws HardwareException, InterruptedException { - moveTubeRackTo(getTExistJudgXPos() + tubeIndex * getTubeSpacing()); + moveTubeRackTo(getTExistJudgXPos() + tubeIndex * getTubeSpacing(), TargetPosMeasureDirection.NEGATIVE, false); return AppRet.success(isTubeExist()); }