From d6f78b16d13fb578423ede27d7cf1c6fcbd2b8ee Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 22 Jul 2024 14:47:21 +0800 Subject: [PATCH] fix some bug --- src/main/java/a8k/a8k_can_protocol/A8kEcode.java | 3 +- src/main/java/a8k/a8k_can_protocol/A8kPacket.java | 24 +++++--- src/main/java/a8k/a8k_can_protocol/MId.java | 65 ++++++--------------- .../java/a8k/base_hardware/A8kCanBusService.java | 25 ++++++-- .../DeviceInitializationCtrlService.java | 26 +++++---- src/main/java/a8k/utils/ByteArray.java | 6 +- zhaohe_app.db | Bin 479232 -> 479232 bytes 7 files changed, 74 insertions(+), 75 deletions(-) diff --git a/src/main/java/a8k/a8k_can_protocol/A8kEcode.java b/src/main/java/a8k/a8k_can_protocol/A8kEcode.java index 27af050..4f64aa6 100644 --- a/src/main/java/a8k/a8k_can_protocol/A8kEcode.java +++ b/src/main/java/a8k/a8k_can_protocol/A8kEcode.java @@ -20,6 +20,8 @@ public enum A8kEcode { PusherMInitPosError(14, "板夹仓推杆电机没有处于零点位置"),// RecycleBinOverflow(15, "回收仓满"),// MotorLostStep(16, "电机丢步"),// + ActionOvertime(17, "动作执行超时"),// + // // FOR HARDWARE @@ -39,7 +41,6 @@ public enum A8kEcode { CmdParamNumError(1114, "CmdParamNumError"),// CheckcodeIsError(1115, "CheckcodeIsError"),// IllegalOperation(1116, "IllegalOperation"),// - ActionOvertime(1117, "ActionOvertime"),// ModuleOpeationBreakByUser(1202, "ModuleOpeationBreakByUser"),// ModuleNotFindReg(1207, "ModuleNotFindReg"),// XYMotorXFindZeroEdgeFail(1306, "XYMotorXFindZeroEdgeFail"),// diff --git a/src/main/java/a8k/a8k_can_protocol/A8kPacket.java b/src/main/java/a8k/a8k_can_protocol/A8kPacket.java index 97fddb2..77831cc 100644 --- a/src/main/java/a8k/a8k_can_protocol/A8kPacket.java +++ b/src/main/java/a8k/a8k_can_protocol/A8kPacket.java @@ -1,6 +1,8 @@ package a8k.a8k_can_protocol; import a8k.utils.ByteArray; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; public class A8kPacket { /** @@ -18,6 +20,8 @@ public class A8kPacket { * kptv2_error_ack = 2, * kptv2_event = 3, */ + public static final Logger logger = LoggerFactory.getLogger(A8kPacket.class); + byte[] raw; @@ -63,6 +67,9 @@ public class A8kPacket { } public byte[] getCmdContent() { + if (raw.length < 8) { + return new byte[0]; + } byte[] cmdcontent = new byte[raw.length - 8]; System.arraycopy(raw, 8, cmdcontent, 0, raw.length - 8); return cmdcontent; @@ -82,21 +89,20 @@ public class A8kPacket { CmdId cmdId = CmdId.valueOf(getCmdId()); assert cmdId != null; - if (packetType == PACKET_TYPE_CMD) { if (cmdId.cmdAttachType == CmdId.ATTACH_IS_INT32) { - ret = String.format("[CMD ] index:[%d] (%s %s :i32[%s])", getPacketIndex(), cmdId, MId.valueOf(getModuleId()), formatInt32ATTACH(getCmdContent())); + ret = String.format("[CMD ] index:[%d] (%s %s(%d) :param:[%s])", getPacketIndex(), cmdId, MId.valueOf(getModuleId()), getModuleId(), formatInt32ATTACH(getCmdContent())); } else { - ret = String.format("[CMD ] index:[%d] (%s %s(%d) :bytes[%s])", getPacketIndex(), cmdId, MId.valueOf(getModuleId()), getModuleId(), ByteArray.toByteString(getCmdContent())); + ret = String.format("[CMD ] index:[%d] (%s %s(%d) :param:[%s])", getPacketIndex(), cmdId, MId.valueOf(getModuleId()), getModuleId(), ByteArray.toByteString(getCmdContent())); } } else if (packetType == PACKET_TYPE_ACK) { if (cmdId.receiptAttachType == CmdId.ATTACH_IS_INT32) { - ret = String.format("[ACK ] index:[%d] (%s %s(%d) :[%s])", getPacketIndex(), cmdId, MId.valueOf(getModuleId()), getModuleId(), formatInt32ATTACH(getCmdContent())); + ret = String.format("[ACK ] index:[%d] (%s :[%s])", getPacketIndex(), cmdId, formatInt32ATTACH(getCmdContent())); } else { - ret = String.format("[ACK ] index:[%d] (%s %s(%d) :[%s])", getPacketIndex(), cmdId, MId.valueOf(getModuleId()), getModuleId(), ByteArray.toByteString(getCmdContent())); + ret = String.format("[ACK ] index:[%d] (%s :[%s])", getPacketIndex(), cmdId, ByteArray.toByteString(getCmdContent())); } } else if (packetType == PACKET_TYPE_ERROR_ACK) { - ret = String.format("[EACK ] index:[%d] (%s %s(%d) :[%s])", getPacketIndex(), cmdId, MId.valueOf(getModuleId()), getModuleId(), A8kEcode.toString(getContentI32(0))); + ret = String.format("[EACK ] index:[%d] (%s :[%s])", getPacketIndex(), cmdId, A8kEcode.toString(getContentI32(0))); } else if (packetType == PACKET_TYPE_EVENT) { if (cmdId.cmdAttachType == CmdId.ATTACH_IS_INT32) { ret = String.format("[EVENT] index:[%d] (%s %s(%d) :[%s])", getPacketIndex(), cmdId, MId.valueOf(getModuleId()), getModuleId(), formatInt32ATTACH(getCmdContent())); @@ -112,9 +118,11 @@ public class A8kPacket { private String formatInt32ATTACH(byte[] attach) { StringBuilder ret = new StringBuilder(); for (int i = 0; i < attach.length; i += 4) { - if (i + 4 >= attach.length) + if (i + 3 >= attach.length) break; - ret.append(String.format("%d ", ByteArray.read32bit(attach, i))); + if (i != 0) + ret.append(","); + ret.append(String.format("%d", ByteArray.read32bit(attach, i))); } return ret.toString(); } diff --git a/src/main/java/a8k/a8k_can_protocol/MId.java b/src/main/java/a8k/a8k_can_protocol/MId.java index f74a650..6802035 100644 --- a/src/main/java/a8k/a8k_can_protocol/MId.java +++ b/src/main/java/a8k/a8k_can_protocol/MId.java @@ -68,53 +68,22 @@ public enum MId { } public static MId valueOf(int val) { - return switch (val) { - case 10 -> HbotBoard; - case 11 -> HbotM; - // - case 20 -> PlatesBoxBoard; - case 21 -> PlatesBoxYM; - case 22 -> PlatesBoxPusherM; - case 23 -> PlatesBoxScanner; - // - case 30 -> ShakeModBoard; - case 31 -> ShakeModClampingM; - case 32 -> ShakeModGripperZM; - case 33 -> ShakeModShakeM; - case 34 -> ShakeModGripperYM; - case 35 -> ShakeModGripperSV; - case 36 -> ShakeModTubeScanerClampingSV; - case 37 -> ShakeModTubeScanerRotateSV; - // - case 40 -> PlatesBoxTCMBoard; - case 41 -> PlatesBoxTCM; - case 42 -> WbTubeFanMod; - // - case 50 -> IncubatorTCMBoard; - case 51 -> IncubatorTCM; - // - case 60 -> FeedingModBoard; - case 61 -> FeedingModInfeedM; - case 62 -> FeedingModXM; - case 63 -> FeedingModOutfeedM; - case 64 -> FeedingModScannerMod; - // - case 70 -> IncubatorRotateBoard; - case 71 -> IncubatorRotateCtrlM; - // - case 80 -> PipetteModBoard; - case 81 -> PipetteModZM; - case 82 -> PipetteMod; - case 83 -> PipetteModCodeScanner; - // - case 90 -> OptModBoard; - case 91 -> OptModScannerM; - case 92 -> OptModPullM; - case 93 -> OptMod; - // - case 100 -> A8kIdCardReaderBoard; - case 101 -> A8kIdCardReader; - default -> NotSet; - }; + MId[] values = MId.values(); + for (MId e : values) { + if (e.toInt() == val) + return e; + } + return null; } + + public static String toString(int val) { + MId[] values = MId.values(); + for (MId e : values) { + if (e.toInt() == val) { + return e.toString(); + } + } + return "unkown(" + val + ")"; + } + } diff --git a/src/main/java/a8k/base_hardware/A8kCanBusService.java b/src/main/java/a8k/base_hardware/A8kCanBusService.java index f7393f8..8e0e9b7 100644 --- a/src/main/java/a8k/base_hardware/A8kCanBusService.java +++ b/src/main/java/a8k/base_hardware/A8kCanBusService.java @@ -20,6 +20,8 @@ import java.net.URI; import java.net.URISyntaxException; import java.nio.ByteBuffer; import java.nio.ByteOrder; +import java.util.HashMap; +import java.util.Map; import java.util.Timer; import java.util.TimerTask; import java.util.concurrent.BlockingQueue; @@ -70,6 +72,9 @@ public class A8kCanBusService { // websocket自动重连时间 Timer autoConnectTimer = new Timer();// + + Map lastCmdMap = new HashMap<>(); + @PostConstruct public void init() throws URISyntaxException { @@ -372,21 +377,26 @@ public class A8kCanBusService { public void waitForMod(MId mid, Integer acitionOvertime) throws InterruptedException, HardwareException { long startedAt = System.currentTimeMillis(); + + CmdId action = lastCmdMap.get(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) { } long now = System.currentTimeMillis(); if (now - startedAt > acitionOvertime) { - throw new HardwareException(mid, A8kEcode.Overtime); + logger.error("{} waitting for action {} overtime", mid, action); + throw new HardwareException(mid, A8kEcode.ActionOvertime); } + Thread.sleep(100); } while (true); @@ -455,9 +465,7 @@ public class A8kCanBusService { try { return this.sendCmd(pack, A8kPacket.CMD_OVERTIME); } catch (HardwareException e) { - if (e.getErrorCode() != A8kEcode.Overtime) - ; - { + if (e.getErrorCode() != A8kEcode.Overtime) { throw e; } } @@ -502,8 +510,15 @@ public class A8kCanBusService { String txpacket = pack.toByteString(); if (debugFlag && pack.getCmdId() != CmdId.module_get_status.toInt()) { - logger.info("Tx:|RAW:{}| {}", txpacket, pack); + String packstr = pack.toString(); + logger.info("Tx:|RAW:{}| {}", txpacket, packstr); } + + if (pack.getCmdId() != CmdId.module_get_status.toInt()) { + lastCmdMap.put(MId.valueOf(pack.getModuleId()), CmdId.valueOf(pack.getCmdId())); + } + + client.send(txpacket); A8kPacket receipt; try { diff --git a/src/main/java/a8k/service/ctrl_service/DeviceInitializationCtrlService.java b/src/main/java/a8k/service/ctrl_service/DeviceInitializationCtrlService.java index 4ab01b0..212c39d 100644 --- a/src/main/java/a8k/service/ctrl_service/DeviceInitializationCtrlService.java +++ b/src/main/java/a8k/service/ctrl_service/DeviceInitializationCtrlService.java @@ -1,6 +1,7 @@ package a8k.service.ctrl_service; import a8k.a8k_can_protocol.A8kEcode; +import a8k.a8k_can_protocol.CmdId; import a8k.a8k_can_protocol.IOId; import a8k.a8k_can_protocol.MId; import a8k.appbean.HardwareException; @@ -33,6 +34,7 @@ public class DeviceInitializationCtrlService { public AppEcode initializeDevice() { logger.info("Initializing device ..."); + boolean initSuc = false; try { /* * 复位思路 @@ -62,6 +64,8 @@ public class DeviceInitializationCtrlService { } // 清空耗材 + initSuc = true; + } catch (HardwareException e) { logger.error("HardwareException ", e); @@ -70,12 +74,14 @@ public class DeviceInitializationCtrlService { logger.error("InterruptedException", e); throw new RuntimeException(e); } finally { - try { - canBus.stepMotorEnable(MId.ShakeModClampingM, 1); - canBus.stepMotorEasyMoveBy(MId.ShakeModClampingM, 2); - canBus.waitForMod(MId.ShakeModClampingM, actionOvertime); - commonHardOpera.forceStopAndDisableAllMOtor(); - } catch (HardwareException | InterruptedException ignored) { + if (!initSuc) { + try { + canBus.stepMotorEnable(MId.ShakeModClampingM, 1); + canBus.stepMotorEasyMoveBy(MId.ShakeModClampingM, 2); + canBus.waitForMod(MId.ShakeModClampingM, actionOvertime); + commonHardOpera.forceStopAndDisableAllMOtor(); + } catch (HardwareException | InterruptedException ignored) { + } } } @@ -145,12 +151,12 @@ public class DeviceInitializationCtrlService { private AppEcode moveMotorToZero() throws HardwareException, InterruptedException { //进出料初始化 -// canBus.stepMotorEasyMoveToZero(MId.FeedingModInfeedM); -// canBus.waitForMod(MId.FeedingModInfeedM, actionOvertime); + // canBus.stepMotorEasyMoveToZero(MId.FeedingModInfeedM); + // canBus.waitForMod(MId.FeedingModInfeedM, actionOvertime); canBus.stepMotorEasyMoveToZero(MId.FeedingModXM); canBus.waitForMod(MId.FeedingModXM, actionOvertime); -// canBus.stepMotorEasyMoveToZero(MId.FeedingModOutfeedM); -// canBus.waitForMod(MId.FeedingModOutfeedM, actionOvertime); + // canBus.stepMotorEasyMoveToZero(MId.FeedingModOutfeedM); + // canBus.waitForMod(MId.FeedingModOutfeedM, actionOvertime); //摇匀模组初始化 diff --git a/src/main/java/a8k/utils/ByteArray.java b/src/main/java/a8k/utils/ByteArray.java index 355dafb..2028794 100644 --- a/src/main/java/a8k/utils/ByteArray.java +++ b/src/main/java/a8k/utils/ByteArray.java @@ -21,7 +21,7 @@ public class ByteArray { } public static int readU16bit(byte[] code, int index) { - if (index + 2 >= code.length) + if (index + 1 >= code.length) return 0; return (code[index + 1] & 255) << 8 | code[index] & 255; } @@ -32,14 +32,14 @@ public class ByteArray { } public static int readS16bit(byte[] code, int index) { - if (index + 2 >= code.length) + if (index + 1 >= code.length) return 0; return code[index + 1] << 8 | code[index] & 255; } public static int read32bit(byte[] code, int index) { - if (index + 4 >= code.length) + if (index + 3 >= code.length) return 0; return code[index + 3] << 24 | (code[index + 2] & 255) << 16 | (code[index + 1] & 255) << 8 | code[index] & 255; } diff --git a/zhaohe_app.db b/zhaohe_app.db index a7135d182cce149d8f974e4f46c8a16eecda630d..227cae229f6900d86aa7d47316de31b20398ea38 100644 GIT binary patch delta 121 zcmZozAltA&c7n8EECT~WDGhIDG